diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000..b41fae9
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,47 @@
+# Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AllowShortFunctionsOnASingleLine: InlineOnly
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: true
+IncludeCategories:
+  # C++ system headers
+  - Regex: <[a-z_]*>
+    Priority: 6
+    CaseSensitive: true
+  # C system headers
+  - Regex: <.*\.h>
+    Priority: 5
+    CaseSensitive: true
+  # Boost headers
+  - Regex: boost/.*
+    Priority: 4
+    CaseSensitive: true
+  # Message headers
+  - Regex: .*_msgs/.*
+    Priority: 3
+    CaseSensitive: true
+  - Regex: .*_srvs/.*
+    Priority: 3
+    CaseSensitive: true
+  # Other Package headers
+  - Regex: <.*>
+    Priority: 2
+    CaseSensitive: true
+  # Local package headers
+  - Regex: '".*"'
+    Priority: 1
+    CaseSensitive: true
diff --git a/.clang-tidy b/.clang-tidy
new file mode 100644
index 0000000..c4533f5
--- /dev/null
+++ b/.clang-tidy
@@ -0,0 +1,461 @@
+Checks: "
+  -*,
+  boost-use-to-string,
+  bugprone-argument-comment,
+  bugprone-assert-side-effect,
+  bugprone-bad-signal-to-kill-thread,
+  bugprone-bool-pointer-implicit-conversion,
+  bugprone-branch-clone,
+  bugprone-copy-constructor-init,
+  bugprone-dangling-handle,
+  bugprone-dynamic-static-initializers,
+  bugprone-exception-escape,
+  bugprone-fold-init-type,
+  bugprone-forward-declaration-namespace,
+  bugprone-forwarding-reference-overload,
+  bugprone-inaccurate-erase,
+  bugprone-incorrect-roundings,
+  bugprone-integer-division,
+  bugprone-lambda-function-name,
+  bugprone-macro-parentheses,
+  bugprone-macro-repeated-side-effects,
+  bugprone-misplaced-operator-in-strlen-in-alloc,
+  bugprone-misplaced-widening-cast,
+  bugprone-move-forwarding-reference,
+  bugprone-multiple-statement-macro,
+  bugprone-not-null-terminated-result,
+  bugprone-parent-virtual-call,
+  bugprone-posix-return,
+  bugprone-signed-char-misuse,
+  bugprone-sizeof-container,
+  bugprone-sizeof-expression,
+  bugprone-string-constructor,
+  bugprone-string-integer-assignment,
+  bugprone-string-literal-with-embedded-nul,
+  bugprone-suspicious-enum-usage,
+  bugprone-suspicious-memset-usage,
+  bugprone-suspicious-missing-comma,
+  bugprone-suspicious-semicolon,
+  bugprone-suspicious-string-compare,
+  bugprone-swapped-arguments,
+  bugprone-terminating-continue,
+  bugprone-throw-keyword-missing,
+  bugprone-too-small-loop-variable,
+  bugprone-unchecked-optional-access,
+  bugprone-undefined-memory-manipulation,
+  bugprone-undelegated-constructor,
+  bugprone-unhandled-self-assignment,
+  bugprone-unused-raii,
+  bugprone-unused-return-value,
+  bugprone-use-after-move,
+  bugprone-virtual-near-miss,
+  cppcoreguidelines-avoid-goto,
+  cppcoreguidelines-init-variables,
+  cppcoreguidelines-interfaces-global-init,
+  cppcoreguidelines-macro-usage,
+  cppcoreguidelines-narrowing-conversions,
+  cppcoreguidelines-no-malloc,
+  cppcoreguidelines-pro-bounds-pointer-arithmetic,
+  cppcoreguidelines-pro-type-const-cast,
+  cppcoreguidelines-pro-type-cstyle-cast,
+  cppcoreguidelines-pro-type-member-init,
+  cppcoreguidelines-pro-type-reinterpret-cast,
+  cppcoreguidelines-pro-type-static-cast-downcast,
+  cppcoreguidelines-pro-type-union-access,
+  cppcoreguidelines-slicing,
+  cppcoreguidelines-special-member-functions,
+  google-build-explicit-make-pair,
+  google-build-namespaces,
+  google-build-using-namespace,
+  google-explicit-constructor,
+  google-global-names-in-headers,
+  google-upgrade-googletest-case,
+  hicpp-exception-baseclass,
+  hicpp-multiway-paths-covered,
+  hicpp-no-assembler,
+  hicpp-signed-bitwise,
+  llvm-namespace-comment,
+  misc-definitions-in-headers,
+  misc-misplaced-const,
+  misc-new-delete-overloads,
+  misc-non-copyable-objects,
+  misc-redundant-expression,
+  misc-static-assert,
+  misc-throw-by-value-catch-by-reference,
+  misc-unconventional-assign-operator,
+  misc-uniqueptr-reset-release,
+  misc-unused-alias-decls,
+  misc-unused-parameters,
+  misc-unused-using-decls,
+  modernize-concat-nested-namespaces,
+  modernize-deprecated-headers,
+  modernize-deprecated-ios-base-aliases,
+  modernize-loop-convert,
+  modernize-make-shared,
+  modernize-make-unique,
+  modernize-pass-by-value,
+  modernize-raw-string-literal,
+  modernize-redundant-void-arg,
+  modernize-replace-auto-ptr,
+  modernize-replace-disallow-copy-and-assign-macro,
+  modernize-replace-random-shuffle,
+  modernize-return-braced-init-list,
+  modernize-shrink-to-fit,
+  modernize-unary-static-assert,
+  modernize-use-auto,
+  modernize-use-bool-literals,
+  modernize-use-default-member-init,
+  modernize-use-emplace,
+  modernize-use-equals-default,
+  modernize-use-equals-delete,
+  modernize-use-nodiscard,
+  modernize-use-noexcept,
+  modernize-use-nullptr,
+  modernize-use-override,
+  modernize-use-transparent-functors,
+  modernize-use-uncaught-exceptions,
+  modernize-use-using,
+  openmp-use-default-none,
+  performance-faster-string-find,
+  performance-for-range-copy,
+  performance-implicit-conversion-in-loop,
+  performance-inefficient-algorithm,
+  performance-inefficient-string-concatenation,
+  performance-inefficient-vector-operation,
+  performance-move-const-arg,
+  performance-move-constructor-init,
+  performance-no-automatic-move,
+  performance-no-int-to-ptr,
+  performance-noexcept-move-constructor,
+  performance-trivially-destructible,
+  performance-type-promotion-in-math-fn,
+  performance-unnecessary-copy-initialization,
+  performance-unnecessary-value-param,
+  portability-simd-intrinsics,
+  readability-const-return-type,
+  readability-container-size-empty,
+  readability-convert-member-functions-to-static,
+  readability-delete-null-pointer,
+  readability-else-after-return,
+  readability-function-cognitive-complexity,
+  readability-identifier-naming,
+  readability-inconsistent-declaration-parameter-name,
+  readability-isolate-declaration,
+  readability-make-member-function-const,
+  readability-misleading-indentation,
+  readability-misplaced-array-index,
+  readability-non-const-parameter,
+  readability-redundant-access-specifiers,
+  readability-redundant-control-flow,
+  readability-redundant-declaration,
+  readability-redundant-function-ptr-dereference,
+  readability-redundant-member-init,
+  readability-redundant-smartptr-get,
+  readability-redundant-string-cstr,
+  readability-redundant-string-init,
+  readability-simplify-boolean-expr,
+  readability-simplify-subscript-expr,
+  readability-static-accessed-through-instance,
+  readability-static-definition-in-anonymous-namespace,
+  readability-string-compare,
+  readability-uniqueptr-delete-release"
+
+WarningsAsErrors: "
+  boost-use-to-string,
+  bugprone-dangling-handle,
+  bugprone-fold-init-type,
+  bugprone-inaccurate-erase,
+  bugprone-incorrect-roundings,
+  bugprone-misplaced-widening-cast,
+  bugprone-sizeof-container,
+  bugprone-sizeof-expression,
+  bugprone-string-constructor,
+  bugprone-suspicious-enum-usage,
+  bugprone-suspicious-memset-usage,
+  bugprone-suspicious-missing-comma,
+  bugprone-suspicious-semicolon,
+  bugprone-swapped-arguments,
+  bugprone-unused-raii,
+  bugprone-use-after-move,
+  llvm-namespace-comment,
+  misc-non-copyable-objects,
+  misc-redundant-expression,
+  misc-throw-by-value-catch-by-reference,
+  misc-unused-alias-decls,
+  misc-unused-parameters,
+  misc-unused-using-decls,
+  modernize-deprecated-headers,
+  modernize-redundant-void-arg,
+  modernize-use-bool-literals,
+  modernize-use-emplace,
+  modernize-use-equals-default,
+  modernize-use-equals-delete,
+  modernize-use-nullptr,
+  modernize-use-override,
+  modernize-use-using,
+  performance-faster-string-find,
+  performance-inefficient-algorithm,
+  readability-make-member-function-const,
+  readability-misleading-indentation,
+  readability-misplaced-array-index,
+  readability-string-compare"
+
+HeaderFilterRegex: ^(?!\/usr)(?!\/opt)
+
+AnalyzeTemporaryDtors: false
+
+FormatStyle: none
+
+CheckOptions:
+  - key: bugprone-argument-comment.CommentBoolLiterals
+    value: "0"
+  - key: bugprone-argument-comment.CommentCharacterLiterals
+    value: "0"
+  - key: bugprone-argument-comment.CommentFloatLiterals
+    value: "0"
+  - key: bugprone-argument-comment.CommentIntegerLiterals
+    value: "0"
+  - key: bugprone-argument-comment.CommentNullPtrs
+    value: "0"
+  - key: bugprone-argument-comment.CommentStringLiterals
+    value: "0"
+  - key: bugprone-argument-comment.CommentUserDefinedLiterals
+    value: "0"
+  - key: bugprone-argument-comment.IgnoreSingleArgument
+    value: "0"
+  - key: bugprone-argument-comment.StrictMode
+    value: "0"
+  - key: bugprone-assert-side-effect.AssertMacros
+    value: assert
+  - key: bugprone-assert-side-effect.CheckFunctionCalls
+    value: "0"
+  - key: bugprone-dangling-handle.HandleClasses
+    value: std::basic_string_view;std::experimental::basic_string_view
+  - key: bugprone-dynamic-static-initializers.HeaderFileExtensions
+    value: ",h,hh,hpp,hxx"
+  - key: bugprone-exception-escape.FunctionsThatShouldNotThrow
+    value: ""
+  - key: bugprone-exception-escape.IgnoredExceptions
+    value: ""
+  - key: bugprone-misplaced-widening-cast.CheckImplicitCasts
+    value: "0"
+  - key: bugprone-not-null-terminated-result.WantToUseSafeFunctions
+    value: "1"
+  - key: bugprone-signed-char-misuse.CharTypdefsToIgnore
+    value: ""
+  - key: bugprone-sizeof-expression.WarnOnSizeOfCompareToConstant
+    value: "1"
+  - key: bugprone-sizeof-expression.WarnOnSizeOfConstant
+    value: "1"
+  - key: bugprone-sizeof-expression.WarnOnSizeOfIntegerExpression
+    value: "0"
+  - key: bugprone-sizeof-expression.WarnOnSizeOfThis
+    value: "1"
+  - key: bugprone-string-constructor.LargeLengthThreshold
+    value: "8388608"
+  - key: bugprone-string-constructor.WarnOnLargeLength
+    value: "1"
+  - key: bugprone-suspicious-enum-usage.StrictMode
+    value: "0"
+  - key: bugprone-suspicious-missing-comma.MaxConcatenatedTokens
+    value: "5"
+  - key: bugprone-suspicious-missing-comma.RatioThreshold
+    value: "0.200000"
+  - key: bugprone-suspicious-missing-comma.SizeThreshold
+    value: "5"
+  - key: bugprone-suspicious-string-compare.StringCompareLikeFunctions
+    value: ""
+  - key: bugprone-suspicious-string-compare.WarnOnImplicitComparison
+    value: "1"
+  - key: bugprone-suspicious-string-compare.WarnOnLogicalNotComparison
+    value: "0"
+  - key: bugprone-too-small-loop-variable.MagnitudeBitsUpperLimit
+    value: "16"
+  - key: bugprone-unhandled-self-assignment.WarnOnlyIfThisHasSuspiciousField
+    value: "1"
+  - key: bugprone-unused-return-value.CheckedFunctions
+    value: ::std::async;::std::launder;::std::remove;::std::remove_if;::std::unique;::std::unique_ptr::release;::std::basic_string::empty;::std::vector::empty
+  - key: cert-dcl16-c.NewSuffixes
+    value: L;LL;LU;LLU
+  - key: cert-oop54-cpp.WarnOnlyIfThisHasSuspiciousField
+    value: "0"
+  - key: cppcoreguidelines-explicit-virtual-functions.IgnoreDestructors
+    value: "1"
+  - key: cppcoreguidelines-macro-usage.AllowedRegexp
+    value: ^DEBUG_*
+  - key: cppcoreguidelines-macro-usage.CheckCapsOnly
+    value: "0"
+  - key: cppcoreguidelines-macro-usage.IgnoreCommandLineMacros
+    value: "1"
+  - key: cppcoreguidelines-no-malloc.Allocations
+    value: ::malloc;::calloc
+  - key: cppcoreguidelines-no-malloc.Deallocations
+    value: ::free
+  - key: cppcoreguidelines-no-malloc.Reallocations
+    value: ::realloc
+  - key: cppcoreguidelines-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic
+    value: "1"
+  - key: cppcoreguidelines-pro-type-member-init.IgnoreArrays
+    value: "0"
+  - key: cppcoreguidelines-pro-type-member-init.UseAssignment
+    value: "0"
+  - key: cppcoreguidelines-special-member-functions.AllowMissingMoveFunctions
+    value: "0"
+  - key: cppcoreguidelines-special-member-functions.AllowSoleDefaultDtor
+    value: "0"
+  - key: google-readability-braces-around-statements.ShortStatementLines
+    value: "1"
+  - key: google-readability-function-size.StatementThreshold
+    value: "800"
+  - key: google-readability-namespace-comments.ShortNamespaceLines
+    value: "10"
+  - key: google-readability-namespace-comments.SpacesBeforeComments
+    value: "2"
+  - key: hicpp-multiway-paths-covered.WarnOnMissingElse
+    value: "0"
+  - key: hicpp-signed-bitwise.IgnorePositiveIntegerLiterals
+    value: "0"
+  - key: misc-definitions-in-headers.HeaderFileExtensions
+    value: ",h,hh,hpp,hxx"
+  - key: misc-definitions-in-headers.UseHeaderFileExtension
+    value: "1"
+  - key: misc-throw-by-value-catch-by-reference.CheckThrowTemporaries
+    value: "1"
+  - key: misc-unused-parameters.StrictMode
+    value: "0"
+  - key: modernize-loop-convert.MaxCopySize
+    value: "16"
+  - key: modernize-loop-convert.MinConfidence
+    value: reasonable
+  - key: modernize-loop-convert.NamingStyle
+    value: CamelCase
+  - key: modernize-make-shared.IgnoreMacros
+    value: "1"
+  - key: modernize-make-shared.IncludeStyle
+    value: google
+  - key: modernize-make-shared.MakeSmartPtrFunction
+    value: std::make_shared
+  - key: modernize-make-shared.MakeSmartPtrFunctionHeader
+    value: memory
+  - key: modernize-make-unique.IgnoreMacros
+    value: "1"
+  - key: modernize-make-unique.IncludeStyle
+    value: google
+  - key: modernize-make-unique.MakeSmartPtrFunction
+    value: std::make_unique
+  - key: modernize-make-unique.MakeSmartPtrFunctionHeader
+    value: memory
+  - key: modernize-pass-by-value.IncludeStyle
+    value: google
+  - key: modernize-pass-by-value.ValuesOnly
+    value: "0"
+  - key: modernize-raw-string-literal.ReplaceShorterLiterals
+    value: "0"
+  - key: modernize-replace-auto-ptr.IncludeStyle
+    value: google
+  - key: modernize-replace-random-shuffle.IncludeStyle
+    value: google
+  - key: modernize-use-auto.MinTypeNameLength
+    value: "5"
+  - key: modernize-use-auto.RemoveStars
+    value: "0"
+  - key: modernize-use-default-member-init.IgnoreMacros
+    value: "1"
+  - key: modernize-use-default-member-init.UseAssignment
+    value: "0"
+  - key: modernize-use-emplace.ContainersWithPushBack
+    value: ::std::vector;::std::list;::std::deque
+  - key: modernize-use-emplace.SmartPointers
+    value: ::std::shared_ptr;::std::unique_ptr;::std::auto_ptr;::std::weak_ptr
+  - key: modernize-use-emplace.TupleMakeFunctions
+    value: ::std::make_pair;::std::make_tuple
+  - key: modernize-use-emplace.TupleTypes
+    value: ::std::pair;::std::tuple
+  - key: modernize-use-equals-default.IgnoreMacros
+    value: "1"
+  - key: modernize-use-equals-delete.IgnoreMacros
+    value: "1"
+  - key: modernize-use-nodiscard.ReplacementString
+    value: "[[nodiscard]]"
+  - key: modernize-use-noexcept.ReplacementString
+    value: ""
+  - key: modernize-use-noexcept.UseNoexceptFalse
+    value: "1"
+  - key: modernize-use-nullptr.NullMacros
+    value: "NULL"
+  - key: modernize-use-override.AllowOverrideAndFinal
+    value: "0"
+  - key: modernize-use-override.FinalSpelling
+    value: final
+  - key: modernize-use-override.IgnoreDestructors
+    value: "0"
+  - key: modernize-use-override.OverrideSpelling
+    value: override
+  - key: modernize-use-transparent-functors.SafeMode
+    value: "0"
+  - key: modernize-use-using.IgnoreMacros
+    value: "1"
+  - key: performance-faster-string-find.StringLikeClasses
+    value: std::basic_string
+  - key: performance-for-range-copy.AllowedTypes
+    value: ""
+  - key: performance-for-range-copy.WarnOnAllAutoCopies
+    value: "0"
+  - key: performance-inefficient-string-concatenation.StrictMode
+    value: "0"
+  - key: performance-inefficient-vector-operation.EnableProto
+    value: "0"
+  - key: performance-inefficient-vector-operation.VectorLikeClasses
+    value: ::std::vector
+  - key: performance-move-const-arg.CheckTriviallyCopyableMove
+    value: "1"
+  - key: performance-move-constructor-init.IncludeStyle
+    value: google
+  - key: performance-no-automatic-move.AllowedTypes
+    value: ""
+  - key: performance-type-promotion-in-math-fn.IncludeStyle
+    value: google
+  - key: performance-unnecessary-copy-initialization.AllowedTypes
+    value: ""
+  - key: performance-unnecessary-value-param.AllowedTypes
+    value: .*Ptr;.*SharedFuture
+  - key: performance-unnecessary-value-param.IncludeStyle
+    value: google
+  - key: portability-simd-intrinsics.Std
+    value: ""
+  - key: portability-simd-intrinsics.Suggest
+    value: "0"
+  - key: readability-function-cognitive-complexity.IgnoreMacros
+    value: "1"
+  - key: readability-else-after-return.WarnOnUnfixable
+    value: "1"
+  - key: readability-identifier-naming.NamespaceCase
+    value: lower_case
+  - key: readability-identifier-naming.ClassCase
+    value: CamelCase
+  - key: readability-identifier-naming.PrivateMemberSuffix
+    value: _
+  - key: readability-identifier-naming.StructCase
+    value: CamelCase
+  - key: readability-identifier-naming.FunctionCase
+    value: lower_case
+  - key: readability-identifier-naming.VariableCase
+    value: lower_case
+  - key: readability-identifier-naming.GlobalConstantCase
+    value: lower_case
+  - key: readability-identifier-naming.GlobalConstantPrefix
+    value: g_
+  - key: readability-identifier-naming.ConstexprVariableCase
+    value: lower_case
+  - key: readability-inconsistent-declaration-parameter-name.IgnoreMacros
+    value: "1"
+  - key: readability-inconsistent-declaration-parameter-name.Strict
+    value: "0"
+  - key: readability-redundant-smartptr-get.IgnoreMacros
+    value: "1"
+  - key: readability-redundant-string-init.StringNames
+    value: ::std::basic_string
+  - key: readability-simplify-subscript-expr.Types
+    value: ::std::basic_string;::std::basic_string_view;::std::vector;::std::array
+  - key: readability-static-accessed-through-instance.NameSpecifierNestingThreshold
+    value: "3"
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
new file mode 100644
index 0000000..3be45ab
--- /dev/null
+++ b/.github/CODEOWNERS
@@ -0,0 +1,5 @@
+### Copied from .github/CODEOWNERS-manual ###
+
+### Automatically generated from package.xml ###
+lanelet2_extension/** kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp takayuki.murooka@tier4.jp yutaka.kondo@tier4.jp
+lanelet2_extension_python/** kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp takayuki.murooka@tier4.jp yutaka.kondo@tier4.jp
diff --git a/.github/CODEOWNERS-manual b/.github/CODEOWNERS-manual
new file mode 100644
index 0000000..e69de29
diff --git a/.github/ISSUE_TEMPLATE/bug.yaml b/.github/ISSUE_TEMPLATE/bug.yaml
new file mode 100644
index 0000000..12a8579
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/bug.yaml
@@ -0,0 +1,71 @@
+name: Bug
+description: Report a bug
+body:
+  - type: checkboxes
+    attributes:
+      label: Checklist
+      description: Confirm the following items before proceeding. If one cannot be satisfied, create a discussion thread instead.
+      options:
+        - label: I've read the [contribution guidelines](https://github.com/autowarefoundation/autoware/blob/main/CONTRIBUTING.md).
+          required: true
+        - label: I've searched other issues and no duplicate issues were found.
+          required: true
+        - label: I'm convinced that this is not my fault but a bug.
+          required: true
+
+  - type: textarea
+    attributes:
+      label: Description
+      description: Write a brief description of the bug.
+    validations:
+      required: true
+
+  - type: textarea
+    attributes:
+      label: Expected behavior
+      description: Describe the expected behavior.
+    validations:
+      required: true
+
+  - type: textarea
+    attributes:
+      label: Actual behavior
+      description: Describe the actual behavior.
+    validations:
+      required: true
+
+  - type: textarea
+    attributes:
+      label: Steps to reproduce
+      description: Write the steps to reproduce the bug.
+      placeholder: |-
+        1.
+        2.
+        3.
+    validations:
+      required: true
+
+  - type: textarea
+    attributes:
+      label: Versions
+      description: Provide the version information. You can omit this if you believe it's irrelevant.
+      placeholder: |-
+        - OS:
+        - ROS 2:
+        - Autoware:
+    validations:
+      required: false
+
+  - type: textarea
+    attributes:
+      label: Possible causes
+      description: Write the possible causes if you have any ideas.
+    validations:
+      required: false
+
+  - type: textarea
+    attributes:
+      label: Additional context
+      description: Add any other additional context if it exists.
+    validations:
+      required: false
diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml
new file mode 100644
index 0000000..48765c2
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/config.yml
@@ -0,0 +1,13 @@
+blank_issues_enabled: false
+contact_links:
+  - name: Question
+    url: https://github.com/autowarefoundation/autoware/discussions/new?category=q-a
+    about: Ask a question
+
+  - name: Feature request
+    url: https://github.com/autowarefoundation/autoware/discussions/new?category=feature-requests
+    about: Send a feature request
+
+  - name: Idea
+    url: https://github.com/autowarefoundation/autoware/discussions/new?category=ideas
+    about: Post an idea
diff --git a/.github/ISSUE_TEMPLATE/task.yaml b/.github/ISSUE_TEMPLATE/task.yaml
new file mode 100644
index 0000000..cd8322f
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/task.yaml
@@ -0,0 +1,42 @@
+name: Task
+description: Plan a task
+body:
+  - type: checkboxes
+    attributes:
+      label: Checklist
+      description: Confirm the following items before proceeding. If one cannot be satisfied, create a discussion thread instead.
+      options:
+        - label: I've read the [contribution guidelines](https://github.com/autowarefoundation/autoware/blob/main/CONTRIBUTING.md).
+          required: true
+        - label: I've searched other issues and no duplicate issues were found.
+          required: true
+        - label: I've agreed with the maintainers that I can plan this task.
+          required: true
+
+  - type: textarea
+    attributes:
+      label: Description
+      description: Write a brief description of the task.
+    validations:
+      required: true
+
+  - type: textarea
+    attributes:
+      label: Purpose
+      description: Describe the purpose of the task.
+    validations:
+      required: true
+
+  - type: textarea
+    attributes:
+      label: Possible approaches
+      description: Describe possible approaches for the task.
+    validations:
+      required: true
+
+  - type: textarea
+    attributes:
+      label: Definition of done
+      description: Write the definition of done for the task.
+    validations:
+      required: true
diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
new file mode 100644
index 0000000..97b0e95
--- /dev/null
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -0,0 +1,8 @@
+**Note**: Confirm the [contribution guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/) before submitting a pull request.
+
+Click the `Preview` tab and select a PR template:
+
+- [Standard change](?expand=1&template=standard-change.md)
+- [Small change](?expand=1&template=small-change.md)
+
+**Do NOT send a PR with this description.**
diff --git a/.github/PULL_REQUEST_TEMPLATE/small-change.md b/.github/PULL_REQUEST_TEMPLATE/small-change.md
new file mode 100644
index 0000000..2ff933c
--- /dev/null
+++ b/.github/PULL_REQUEST_TEMPLATE/small-change.md
@@ -0,0 +1,40 @@
+## Description
+
+<!-- Write a brief description of this PR. -->
+
+## Tests performed
+
+<!-- Describe how you have tested this PR. -->
+<!-- Although the default value is set to "Not Applicable.", please update this section if the type is either [feat, fix, perf], or if requested by the reviewers. -->
+
+Not applicable.
+
+## Effects on system behavior
+
+<!-- Describe how this PR affects the system behavior. -->
+
+Not applicable.
+
+## Pre-review checklist for the PR author
+
+The PR author **must** check the checkboxes below when creating the PR.
+
+- [ ] I've confirmed the [contribution guidelines].
+- [ ] The PR follows the [pull request guidelines].
+
+## In-review checklist for the PR reviewers
+
+The PR reviewers **must** check the checkboxes below before approval.
+
+- [ ] The PR follows the [pull request guidelines].
+
+## Post-review checklist for the PR author
+
+The PR author **must** check the checkboxes below before merging.
+
+- [ ] There are no open discussions or they are tracked via tickets.
+
+After all checkboxes are checked, anyone who has write access can merge the PR.
+
+[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/
+[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/
diff --git a/.github/PULL_REQUEST_TEMPLATE/standard-change.md b/.github/PULL_REQUEST_TEMPLATE/standard-change.md
new file mode 100644
index 0000000..7aedefd
--- /dev/null
+++ b/.github/PULL_REQUEST_TEMPLATE/standard-change.md
@@ -0,0 +1,50 @@
+## Description
+
+<!-- Write a brief description of this PR. -->
+
+## Related links
+
+<!-- Write the links related to this PR. Private links should be clearly marked as private, for example, '[FOO COMPANY INTERNAL LINK](https://example.com)'. -->
+
+## Tests performed
+
+<!-- Describe how you have tested this PR. -->
+
+## Notes for reviewers
+
+<!-- Write additional information if necessary. It should be written if there are related PRs that should be merged at the same time. -->
+
+## Interface changes
+
+<!-- Describe any changed interfaces, such as topics, services, or parameters. -->
+
+## Effects on system behavior
+
+<!-- Describe how this PR affects the system behavior. -->
+
+## Pre-review checklist for the PR author
+
+The PR author **must** check the checkboxes below when creating the PR.
+
+- [ ] I've confirmed the [contribution guidelines].
+- [ ] The PR follows the [pull request guidelines].
+
+## In-review checklist for the PR reviewers
+
+The PR reviewers **must** check the checkboxes below before approval.
+
+- [ ] The PR follows the [pull request guidelines].
+- [ ] The PR has been properly tested.
+- [ ] The PR has been reviewed by the code owners.
+
+## Post-review checklist for the PR author
+
+The PR author **must** check the checkboxes below before merging.
+
+- [ ] There are no open discussions or they are tracked via tickets.
+- [ ] The PR is ready for merge.
+
+After all checkboxes are checked, anyone who has write access can merge the PR.
+
+[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/
+[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/
diff --git a/.github/dependabot.yaml b/.github/dependabot.yaml
new file mode 100644
index 0000000..0264c03
--- /dev/null
+++ b/.github/dependabot.yaml
@@ -0,0 +1,10 @@
+version: 2
+updates:
+  - package-ecosystem: github-actions
+    directory: /
+    schedule:
+      interval: daily
+    open-pull-requests-limit: 1
+    labels:
+      - tag:bot
+      - type:github-actions
diff --git a/.github/stale.yml b/.github/stale.yml
new file mode 100644
index 0000000..bc99e43
--- /dev/null
+++ b/.github/stale.yml
@@ -0,0 +1,12 @@
+# Modified from https://github.com/probot/stale#usage
+
+# Number of days of inactivity before an Issue or Pull Request with the stale label is closed
+daysUntilClose: false
+
+# Label to use when marking as stale
+staleLabel: status:stale
+
+# Comment to post when marking as stale
+markComment: >
+  This pull request has been automatically marked as stale because it has not had
+  recent activity.
diff --git a/.github/sync-files-reverse-reference.yaml b/.github/sync-files-reverse-reference.yaml
new file mode 100644
index 0000000..f5d016c
--- /dev/null
+++ b/.github/sync-files-reverse-reference.yaml
@@ -0,0 +1,39 @@
+- repository: autowarefoundation/autoware.universe
+  files:
+    - source: .github/workflows/build-and-test.yaml
+      pre-commands: |
+        sd -s '${{ matrix.container-suffix }}' '' {source}
+        sd -f ms 'container-suffix:.*include:' 'include:' {source}
+
+        sd "container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest.*" "container: ros:\$1" {source}
+        sd "build_depends.humble.repos" "build_depends.repos" {source}
+    - source: .github/workflows/build-and-test-differential.yaml
+      pre-commands: |
+        sd -s '${{ matrix.container-suffix }}' '' {source}
+        sd -f ms 'container-suffix:.*include:' 'include:' {source}
+
+        sd "container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest.*" "container: ros:\$1" {source}
+        sd "build_depends.humble.repos" "build_depends.repos" {source}
+    - source: .github/workflows/build-and-test-differential-self-hosted.yaml
+      pre-commands: |
+        sd -s '${{ matrix.container-suffix }}' '' {source}
+        sd -f ms 'container-suffix:.*include:' 'include:' {source}
+
+        sd "container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest.*" "container: ros:\$1" {source}
+        sd "build_depends.humble.repos" "build_depends.repos" {source}
+    - source: .github/workflows/build-and-test-self-hosted.yaml
+      pre-commands: |
+        sd -s '${{ matrix.container-suffix }}' '' {source}
+        sd -f ms 'container-suffix:.*include:' 'include:' {source}
+
+        sd "container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest.*" "container: ros:\$1" {source}
+        sd "build_depends.humble.repos" "build_depends.repos" {source}
+    - source: .github/workflows/check-build-depends.yaml
+      pre-commands: |
+        sd "container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest.*" "container: ros:\$1" {source}
+        sd "build_depends.humble.repos" "build_depends.repos" {source}
+    - source: .github/workflows/clang-tidy-pr-comments.yaml
+    - source: .github/workflows/clang-tidy-pr-comments-manually.yaml
+    - source: .github/workflows/update-codeowners-from-packages.yaml
+    - source: .pre-commit-config.yaml
+    - source: codecov.yaml
diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml
new file mode 100644
index 0000000..39bcd3f
--- /dev/null
+++ b/.github/sync-files.yaml
@@ -0,0 +1,32 @@
+- repository: autowarefoundation/autoware
+  files:
+    - source: CODE_OF_CONDUCT.md
+    - source: CONTRIBUTING.md
+    - source: DISCLAIMER.md
+    - source: LICENSE
+    - source: .github/ISSUE_TEMPLATE/bug.yaml
+    - source: .github/ISSUE_TEMPLATE/config.yml
+    - source: .github/ISSUE_TEMPLATE/task.yaml
+    - source: .github/PULL_REQUEST_TEMPLATE.md
+    - source: .github/PULL_REQUEST_TEMPLATE/small-change.md
+    - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md
+    - source: .github/dependabot.yaml
+    - source: .github/stale.yml
+    - source: .github/workflows/cancel-previous-workflows.yaml
+    - source: .github/workflows/github-release.yaml
+    - source: .github/workflows/pre-commit.yaml
+    - source: .github/workflows/pre-commit-autoupdate.yaml
+    - source: .github/workflows/pre-commit-optional.yaml
+    - source: .github/workflows/semantic-pull-request.yaml
+    - source: .github/workflows/spell-check-differential.yaml
+    - source: .github/workflows/sync-files.yaml
+    - source: .clang-format
+    - source: .clang-tidy
+    - source: .markdown-link-check.json
+    - source: .markdownlint.yaml
+    - source: .pre-commit-config-optional.yaml
+    - source: .prettierignore
+    - source: .prettierrc.yaml
+    - source: .yamllint.yaml
+    - source: CPPLINT.cfg
+    - source: setup.cfg
diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml
new file mode 100644
index 0000000..b684019
--- /dev/null
+++ b/.github/workflows/build-and-test-differential-self-hosted.yaml
@@ -0,0 +1,58 @@
+name: build-and-test-differential-self-hosted
+
+on:
+  pull_request:
+    types:
+      - opened
+      - synchronize
+      - labeled
+  workflow_dispatch:
+
+jobs:
+  prevent-no-label-execution:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
+    with:
+      label: ARM64
+
+  build-and-test-differential-self-hosted:
+    needs: prevent-no-label-execution
+    if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
+    runs-on: [self-hosted, linux, ARM64]
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - humble
+        include:
+          - rosdistro: humble
+            container: ros:humble
+            build-depends-repos: build_depends.repos
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Build
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Test
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml
new file mode 100644
index 0000000..1179857
--- /dev/null
+++ b/.github/workflows/build-and-test-differential.yaml
@@ -0,0 +1,92 @@
+name: build-and-test-differential
+
+on:
+  pull_request:
+
+jobs:
+  build-and-test-differential:
+    runs-on: ubuntu-latest
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - humble
+        include:
+          - rosdistro: humble
+            container: ros:humble
+            build-depends-repos: build_depends.repos
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Build
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Test
+        id: test
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Upload coverage to CodeCov
+        if: ${{ steps.test.outputs.coverage-report-files != '' }}
+        uses: codecov/codecov-action@v3
+        with:
+          files: ${{ steps.test.outputs.coverage-report-files }}
+          fail_ci_if_error: false
+          verbose: true
+          flags: differential
+
+  clang-tidy-differential:
+    runs-on: ubuntu-latest
+    container: ros:humble
+    needs: build-and-test-differential
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Get modified files
+        id: get-modified-files
+        uses: tj-actions/changed-files@v42
+        with:
+          files: |
+            **/*.cpp
+            **/*.hpp
+
+      # TODO(youtalk): Comment in after fixing clang-tidy error
+      # - name: Run clang-tidy
+      #   if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }}
+      #   uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
+      #   with:
+      #     rosdistro: humble
+      #     target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+      #     target-files: ${{ steps.get-modified-files.outputs.all_changed_files }}
+      #     clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy
+      #     build-depends-repos: build_depends.repos
diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml
new file mode 100644
index 0000000..e1a4c0b
--- /dev/null
+++ b/.github/workflows/build-and-test-self-hosted.yaml
@@ -0,0 +1,46 @@
+name: build-and-test-self-hosted
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  build-and-test-self-hosted:
+    runs-on: [self-hosted, linux, ARM64]
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - humble
+        include:
+          - rosdistro: humble
+            container: ros:humble
+            build-depends-repos: build_depends.repos
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get self packages
+        id: get-self-packages
+        uses: autowarefoundation/autoware-github-actions/get-self-packages@v1
+
+      - name: Build
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Test
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
diff --git a/.github/workflows/build-and-test-with-reverse-depends.yaml b/.github/workflows/build-and-test-with-reverse-depends.yaml
new file mode 100644
index 0000000..fd54629
--- /dev/null
+++ b/.github/workflows/build-and-test-with-reverse-depends.yaml
@@ -0,0 +1,52 @@
+name: build-and-test-with-reverse-depends
+
+on:
+  workflow_dispatch:
+
+jobs:
+  build-and-test-with-reverse-depends:
+    runs-on: ubuntu-latest
+    container: ros:humble
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Set up yq
+        uses: chrisdickinson/setup-yq@v1.0.1
+        with:
+          yq-version: v4.25.1
+
+      - name: Clone reverse depends
+        run: |
+          git clone https://github.com/autowarefoundation/autoware.universe.git reverse_depends
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get self packages
+        id: get-self-packages
+        uses: autowarefoundation/autoware-github-actions/get-self-packages@v1
+
+      - name: Import depends of reverse depends
+        run: |
+          yq -i 'del(.repositories.* | select(.url == "https://github.com/autowarefoundation/autoware_common.git"))' reverse_depends/build_depends.repos
+          yq -i 'del(.repositories.* | select(.url == "https://github.com/tier4/autoware_auto_msgs.git"))' reverse_depends/build_depends.repos
+          vcs import reverse_depends < reverse_depends/build_depends.repos
+          rm -rf reverse_depends/map/lanelet2_extension
+
+      - name: Build
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: humble
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: build_depends.repos
+
+      - name: Test
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        id: test
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
+        with:
+          rosdistro: humble
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: build_depends.repos
diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml
new file mode 100644
index 0000000..c34102c
--- /dev/null
+++ b/.github/workflows/build-and-test.yaml
@@ -0,0 +1,66 @@
+name: build-and-test
+
+on:
+  push:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  build-and-test:
+    if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }}
+    runs-on: ubuntu-latest
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - humble
+        include:
+          - rosdistro: humble
+            container: ros:humble
+            build-depends-repos: build_depends.repos
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Free disk space (Ubuntu)
+        uses: jlumbroso/free-disk-space@v1.3.1
+        with:
+          tool-cache: false
+          dotnet: false
+          swap-storage: false
+          large-packages: false
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get self packages
+        id: get-self-packages
+        uses: autowarefoundation/autoware-github-actions/get-self-packages@v1
+
+      - name: Build
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Test
+        if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
+        id: test
+        uses: autowarefoundation/autoware-github-actions/colcon-test@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Upload coverage to CodeCov
+        if: ${{ steps.test.outputs.coverage-report-files != '' }}
+        uses: codecov/codecov-action@v3
+        with:
+          files: ${{ steps.test.outputs.coverage-report-files }}
+          fail_ci_if_error: false
+          verbose: true
+          flags: total
diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml
new file mode 100644
index 0000000..1da4d24
--- /dev/null
+++ b/.github/workflows/cancel-previous-workflows.yaml
@@ -0,0 +1,14 @@
+name: cancel-previous-workflows
+
+on:
+  pull_request_target:
+
+jobs:
+  cancel-previous-workflows:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Cancel previous runs
+        uses: styfle/cancel-workflow-action@0.12.0
+        with:
+          workflow_id: all
+          all_but_latest: true
diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml
new file mode 100644
index 0000000..81618a1
--- /dev/null
+++ b/.github/workflows/check-build-depends.yaml
@@ -0,0 +1,37 @@
+name: check-build-depends
+
+on:
+  pull_request:
+    paths:
+      - build_depends*.repos
+
+jobs:
+  check-build-depends:
+    runs-on: ubuntu-latest
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - humble
+        include:
+          - rosdistro: humble
+            container: ros:humble
+            build-depends-repos: build_depends.repos
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get self packages
+        id: get-self-packages
+        uses: autowarefoundation/autoware-github-actions/get-self-packages@v1
+
+      - name: Build
+        uses: autowarefoundation/autoware-github-actions/colcon-build@v1
+        with:
+          rosdistro: ${{ matrix.rosdistro }}
+          target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
+          build-depends-repos: ${{ matrix.build-depends-repos }}
diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml
new file mode 100644
index 0000000..9bccd97
--- /dev/null
+++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml
@@ -0,0 +1,62 @@
+name: clang-tidy-pr-comments-manually
+
+on:
+  workflow_dispatch:
+    inputs:
+      workflow_run_id_or_url:
+        description: The target workflow run ID or URL of the build-and-test-differential workflow
+        required: true
+jobs:
+  clang-tidy-pr-comments-manually:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Download analysis results
+        run: |
+          workflow_run_id=$(echo "${{ inputs.workflow_run_id_or_url }}" | sed -e "s|.*runs/||" -e "s|/jobs.*||")
+          gh run download "$workflow_run_id" -D /tmp || true
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Check if the fixes.yaml file exists
+        id: check-fixes-yaml-existence
+        uses: autowarefoundation/autoware-github-actions/check-file-existence@v1
+        with:
+          files: /tmp/clang-tidy-result/fixes.yaml
+
+      - name: Set variables
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        id: set-variables
+        run: |
+          echo "pr-id=$(cat /tmp/clang-tidy-result/pr-id.txt)" >> $GITHUB_OUTPUT
+          echo "pr-head-repo=$(cat /tmp/clang-tidy-result/pr-head-repo.txt)" >> $GITHUB_OUTPUT
+          echo "pr-head-ref=$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" >> $GITHUB_OUTPUT
+
+      - name: Check out PR head
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        uses: actions/checkout@v3
+        with:
+          repository: ${{ steps.set-variables.outputs.pr-head-repo }}
+          ref: ${{ steps.set-variables.outputs.pr-head-ref }}
+          persist-credentials: false
+
+      - name: Replace paths in fixes.yaml
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        run: |
+          sed -i -e "s|/__w/|/home/runner/work/|g" /tmp/clang-tidy-result/fixes.yaml
+          cat /tmp/clang-tidy-result/fixes.yaml
+
+      - name: Copy fixes.yaml to access from Docker Container Action
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        run: |
+          cp /tmp/clang-tidy-result/fixes.yaml fixes.yaml
+
+      - name: Run clang-tidy-pr-comments action
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        uses: platisd/clang-tidy-pr-comments@v1
+        with:
+          github_token: ${{ secrets.GITHUB_TOKEN }}
+          clang_tidy_fixes: fixes.yaml
+          pull_request_id: ${{ steps.set-variables.outputs.pr-id }}
diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml
new file mode 100644
index 0000000..baaa0fb
--- /dev/null
+++ b/.github/workflows/clang-tidy-pr-comments.yaml
@@ -0,0 +1,63 @@
+name: clang-tidy-pr-comments
+
+on:
+  workflow_run:
+    workflows:
+      - build-and-test-differential
+    types:
+      - completed
+
+jobs:
+  clang-tidy-pr-comments:
+    if: ${{ github.event.workflow_run.event == 'pull_request' && contains(fromJson('["success", "failure"]'), github.event.workflow_run.conclusion) }}
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Download analysis results
+        run: |
+          gh run download ${{ github.event.workflow_run.id }} -D /tmp || true
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Check if the fixes.yaml file exists
+        id: check-fixes-yaml-existence
+        uses: autowarefoundation/autoware-github-actions/check-file-existence@v1
+        with:
+          files: /tmp/clang-tidy-result/fixes.yaml
+
+      - name: Set variables
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        id: set-variables
+        run: |
+          echo "pr-id=$(cat /tmp/clang-tidy-result/pr-id.txt)" >> $GITHUB_OUTPUT
+          echo "pr-head-repo=$(cat /tmp/clang-tidy-result/pr-head-repo.txt)" >> $GITHUB_OUTPUT
+          echo "pr-head-ref=$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" >> $GITHUB_OUTPUT
+
+      - name: Check out PR head
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        uses: actions/checkout@v3
+        with:
+          repository: ${{ steps.set-variables.outputs.pr-head-repo }}
+          ref: ${{ steps.set-variables.outputs.pr-head-ref }}
+          persist-credentials: false
+
+      - name: Replace paths in fixes.yaml
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        run: |
+          sed -i -e "s|/__w/|/home/runner/work/|g" /tmp/clang-tidy-result/fixes.yaml
+          cat /tmp/clang-tidy-result/fixes.yaml
+
+      - name: Copy fixes.yaml to access from Docker Container Action
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        run: |
+          cp /tmp/clang-tidy-result/fixes.yaml fixes.yaml
+
+      - name: Run clang-tidy-pr-comments action
+        if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }}
+        uses: platisd/clang-tidy-pr-comments@v1
+        with:
+          github_token: ${{ secrets.GITHUB_TOKEN }}
+          clang_tidy_fixes: fixes.yaml
+          pull_request_id: ${{ steps.set-variables.outputs.pr-id }}
diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml
new file mode 100644
index 0000000..b426d0c
--- /dev/null
+++ b/.github/workflows/github-release.yaml
@@ -0,0 +1,78 @@
+name: github-release
+
+on:
+  push:
+    branches:
+      - beta/v*
+    tags:
+      - v*
+  workflow_dispatch:
+    inputs:
+      beta-branch-or-tag-name:
+        description: The name of the beta branch or tag to release
+        type: string
+        required: true
+
+jobs:
+  github-release:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Set tag name
+        id: set-tag-name
+        run: |
+          if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then
+            REF_NAME="${{ github.event.inputs.beta-branch-or-tag-name }}"
+          else
+            REF_NAME="${{ github.ref_name }}"
+          fi
+
+          echo "ref-name=$REF_NAME" >> $GITHUB_OUTPUT
+          echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT
+
+      - name: Check out repository
+        uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+          ref: ${{ steps.set-tag-name.outputs.ref-name }}
+
+      - name: Set target name for beta branches
+        id: set-target-name
+        run: |
+          if [[ "${{ steps.set-tag-name.outputs.ref-name }}" =~ "beta/" ]]; then
+            echo "target-name=${{ steps.set-tag-name.outputs.ref-name }}" >> $GITHUB_OUTPUT
+          fi
+
+      - name: Create a local tag for beta branches
+        run: |
+          if [ "${{ steps.set-target-name.outputs.target-name }}" != "" ]; then
+            git tag "${{ steps.set-tag-name.outputs.tag-name }}"
+          fi
+
+      - name: Run generate-changelog
+        id: generate-changelog
+        uses: autowarefoundation/autoware-github-actions/generate-changelog@v1
+
+      - name: Select verb
+        id: select-verb
+        run: |
+          has_previous_draft=$(gh release view --json isDraft -q ".isDraft" "${{ steps.set-tag-name.outputs.tag-name }}") || true
+
+          verb=create
+          if [ "$has_previous_draft" = "true" ]; then
+            verb=edit
+          fi
+
+          echo "verb=$verb" >> $GITHUB_OUTPUT
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Release to GitHub
+        run: |
+          gh release ${{ steps.select-verb.outputs.verb }} "${{ steps.set-tag-name.outputs.tag-name }}" \
+            --draft \
+            --target "${{ steps.set-target-name.outputs.target-name }}" \
+            --title "Release ${{ steps.set-tag-name.outputs.tag-name }}" \
+            --notes "$NOTES"
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+          NOTES: ${{ steps.generate-changelog.outputs.changelog }}
diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml
new file mode 100644
index 0000000..23b403f
--- /dev/null
+++ b/.github/workflows/pre-commit-autoupdate.yaml
@@ -0,0 +1,37 @@
+name: pre-commit-autoupdate
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  check-secret:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/check-secret.yaml@v1
+    secrets:
+      secret: ${{ secrets.APP_ID }}
+
+  pre-commit-autoupdate:
+    needs: check-secret
+    if: ${{ needs.check-secret.outputs.set == 'true' }}
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v2
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run pre-commit-autoupdate
+        uses: autowarefoundation/autoware-github-actions/pre-commit-autoupdate@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          pre-commit-config: .pre-commit-config.yaml
+          pr-labels: |
+            tag:bot
+            tag:pre-commit-autoupdate
+          pr-branch: pre-commit-autoupdate
+          pr-title: "ci(pre-commit): autoupdate"
+          pr-commit-message: "ci(pre-commit): autoupdate"
+          auto-merge-method: squash
diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml
new file mode 100644
index 0000000..3873819
--- /dev/null
+++ b/.github/workflows/pre-commit-optional.yaml
@@ -0,0 +1,19 @@
+name: pre-commit-optional
+
+on:
+  pull_request:
+
+jobs:
+  pre-commit-optional:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
+      - name: Run pre-commit
+        uses: autowarefoundation/autoware-github-actions/pre-commit@v1
+        with:
+          pre-commit-config: .pre-commit-config-optional.yaml
+          base-branch: origin/${{ github.base_ref }}
diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml
new file mode 100644
index 0000000..c724885
--- /dev/null
+++ b/.github/workflows/pre-commit.yaml
@@ -0,0 +1,27 @@
+name: pre-commit
+
+on:
+  pull_request:
+
+jobs:
+  pre-commit:
+    if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v2
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Check out repository
+        uses: actions/checkout@v4
+        with:
+          ref: ${{ github.event.pull_request.head.ref }}
+
+      - name: Run pre-commit
+        uses: autowarefoundation/autoware-github-actions/pre-commit@v1
+        with:
+          pre-commit-config: .pre-commit-config.yaml
+          token: ${{ steps.generate-token.outputs.token }}
diff --git a/.github/workflows/semantic-pull-request.yaml b/.github/workflows/semantic-pull-request.yaml
new file mode 100644
index 0000000..71224c2
--- /dev/null
+++ b/.github/workflows/semantic-pull-request.yaml
@@ -0,0 +1,12 @@
+name: semantic-pull-request
+
+on:
+  pull_request_target:
+    types:
+      - opened
+      - edited
+      - synchronize
+
+jobs:
+  semantic-pull-request:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/semantic-pull-request.yaml@v1
diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml
new file mode 100644
index 0000000..1fbf2ff
--- /dev/null
+++ b/.github/workflows/spell-check-differential.yaml
@@ -0,0 +1,16 @@
+name: spell-check-differential
+
+on:
+  pull_request:
+
+jobs:
+  spell-check-differential:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v4
+
+      - name: Run spell-check
+        uses: autowarefoundation/autoware-github-actions/spell-check@v1
+        with:
+          cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json
diff --git a/.github/workflows/sync-files-reverse-reference.yaml b/.github/workflows/sync-files-reverse-reference.yaml
new file mode 100644
index 0000000..8219686
--- /dev/null
+++ b/.github/workflows/sync-files-reverse-reference.yaml
@@ -0,0 +1,24 @@
+name: sync-files-reverse-reference
+
+on:
+  workflow_dispatch:
+
+jobs:
+  sync-files-reverse-reference:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v2
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-files
+        uses: autowarefoundation/autoware-github-actions/sync-files@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          config: .github/sync-files-reverse-reference.yaml
+          pr-branch: sync-files-reverse-reference
+          pr-title: "chore: sync files of reverse references"
+          pr-commit-message: "chore: sync files of reverse references"
diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml
new file mode 100644
index 0000000..51e523b
--- /dev/null
+++ b/.github/workflows/sync-files.yaml
@@ -0,0 +1,33 @@
+name: sync-files
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  check-secret:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/check-secret.yaml@v1
+    secrets:
+      secret: ${{ secrets.APP_ID }}
+
+  sync-files:
+    needs: check-secret
+    if: ${{ needs.check-secret.outputs.set == 'true' }}
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v2
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-files
+        uses: autowarefoundation/autoware-github-actions/sync-files@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          pr-labels: |
+            tag:bot
+            tag:sync-files
+          auto-merge-method: squash
diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml
new file mode 100644
index 0000000..8b3d240
--- /dev/null
+++ b/.github/workflows/update-codeowners-from-packages.yaml
@@ -0,0 +1,33 @@
+name: update-codeowners-from-packages
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  check-secret:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/check-secret.yaml@v1
+    secrets:
+      secret: ${{ secrets.APP_ID }}
+
+  update-codeowners-from-packages:
+    needs: check-secret
+    if: ${{ needs.check-secret.outputs.set == 'true' }}
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v2
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run update-codeowners-from-packages
+        uses: autowarefoundation/autoware-github-actions/update-codeowners-from-packages@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          pr-labels: |
+            tag:bot
+            tag:update-codeowners-from-packages
+          auto-merge-method: squash
diff --git a/.markdown-link-check.json b/.markdown-link-check.json
new file mode 100644
index 0000000..c71a3e4
--- /dev/null
+++ b/.markdown-link-check.json
@@ -0,0 +1,16 @@
+{
+  "aliveStatusCodes": [200, 206, 403],
+  "ignorePatterns": [
+    {
+      "pattern": "^http://localhost"
+    },
+    {
+      "pattern": "^http://127\\.0\\.0\\.1"
+    },
+    {
+      "pattern": "^https://github.com/.*/discussions/new"
+    }
+  ],
+  "retryOn429": true,
+  "retryCount": 10
+}
diff --git a/.markdownlint.yaml b/.markdownlint.yaml
new file mode 100644
index 0000000..babaaa1
--- /dev/null
+++ b/.markdownlint.yaml
@@ -0,0 +1,11 @@
+# See https://github.com/DavidAnson/markdownlint/blob/main/doc/Rules.md for all rules.
+default: true
+MD013: false
+MD024:
+  siblings_only: true
+MD029:
+  style: ordered
+MD033: false
+MD041: false
+MD046: false
+MD049: false
diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml
new file mode 100644
index 0000000..3b43f9a
--- /dev/null
+++ b/.pre-commit-config-optional.yaml
@@ -0,0 +1,6 @@
+repos:
+  - repo: https://github.com/tcort/markdown-link-check
+    rev: v3.11.2
+    hooks:
+      - id: markdown-link-check
+        args: [--quiet, --config=.markdown-link-check.json]
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 0000000..a5ca7b6
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,95 @@
+ci:
+  autofix_commit_msg: "style(pre-commit): autofix"
+
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.4.0
+    hooks:
+      - id: check-json
+      - id: check-merge-conflict
+      - id: check-toml
+      - id: check-xml
+      - id: check-yaml
+        args: [--unsafe]
+      - id: detect-private-key
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        args: [--markdown-linebreak-ext=md]
+
+  - repo: https://github.com/igorshubovych/markdownlint-cli
+    rev: v0.33.0
+    hooks:
+      - id: markdownlint
+        args: [-c, .markdownlint.yaml, --fix]
+
+  - repo: https://github.com/pre-commit/mirrors-prettier
+    rev: v3.0.0-alpha.6
+    hooks:
+      - id: prettier
+
+  - repo: https://github.com/adrienverge/yamllint
+    rev: v1.30.0
+    hooks:
+      - id: yamllint
+
+  - repo: https://github.com/tier4/pre-commit-hooks-ros
+    rev: v0.8.0
+    hooks:
+      - id: flake8-ros
+      - id: prettier-xacro
+      - id: prettier-launch-xml
+      - id: prettier-package-xml
+      - id: ros-include-guard
+      - id: sort-package-xml
+
+  - repo: https://github.com/shellcheck-py/shellcheck-py
+    rev: v0.9.0.2
+    hooks:
+      - id: shellcheck
+
+  - repo: https://github.com/scop/pre-commit-shfmt
+    rev: v3.6.0-2
+    hooks:
+      - id: shfmt
+        args: [-w, -s, -i=4]
+
+  - repo: https://github.com/pycqa/isort
+    rev: 5.12.0
+    hooks:
+      - id: isort
+
+  - repo: https://github.com/psf/black
+    rev: 23.3.0
+    hooks:
+      - id: black
+        args: [--line-length=100]
+
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v16.0.0
+    hooks:
+      - id: clang-format
+        types_or: [c++, c, cuda]
+
+  - repo: https://github.com/cpplint/cpplint
+    rev: 1.6.1
+    hooks:
+      - id: cpplint
+        args: [--quiet]
+        exclude: .cu
+
+  - repo: https://github.com/python-jsonschema/check-jsonschema
+    rev: 0.23.2
+    hooks:
+      - id: check-metaschema
+        files: ^.+/schema/.*schema\.json$
+
+  - repo: local
+    hooks:
+      - id: prettier-svg
+        name: prettier svg
+        description: Apply Prettier with plugin-xml to svg.
+        entry: prettier --write --list-different --ignore-unknown --print-width 200 --xml-self-closing-space false --xml-whitespace-sensitivity ignore
+        language: node
+        files: .svg$
+        additional_dependencies: [prettier@2.7.1, "@prettier/plugin-xml@2.2.0"]
diff --git a/.prettierignore b/.prettierignore
new file mode 100644
index 0000000..a3c34d0
--- /dev/null
+++ b/.prettierignore
@@ -0,0 +1,2 @@
+*.param.yaml
+*.rviz
diff --git a/.prettierrc.yaml b/.prettierrc.yaml
new file mode 100644
index 0000000..e29bf32
--- /dev/null
+++ b/.prettierrc.yaml
@@ -0,0 +1,20 @@
+printWidth: 100
+tabWidth: 2
+overrides:
+  - files: package.xml
+    options:
+      printWidth: 1000
+      xmlSelfClosingSpace: false
+      xmlWhitespaceSensitivity: ignore
+
+  - files: "*.launch.xml"
+    options:
+      printWidth: 200
+      xmlSelfClosingSpace: false
+      xmlWhitespaceSensitivity: ignore
+
+  - files: "*.xacro"
+    options:
+      printWidth: 200
+      xmlSelfClosingSpace: false
+      xmlWhitespaceSensitivity: ignore
diff --git a/.yamllint.yaml b/.yamllint.yaml
new file mode 100644
index 0000000..2c7bd08
--- /dev/null
+++ b/.yamllint.yaml
@@ -0,0 +1,22 @@
+extends: default
+
+ignore: |
+  *.param.yaml
+
+rules:
+  braces:
+    level: error
+    max-spaces-inside: 1 # To format with Prettier
+  comments:
+    level: error
+    min-spaces-from-content: 1 # To be compatible with C++ and Python
+  document-start:
+    level: error
+    present: false # Don't need document start markers
+  line-length: disable # Delegate to Prettier
+  truthy:
+    level: error
+    check-keys: false # To allow 'on' of GitHub Actions
+  quoted-strings:
+    level: error
+    required: only-when-needed # To keep consistent style
diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md
new file mode 100644
index 0000000..8dbcfb8
--- /dev/null
+++ b/CODE_OF_CONDUCT.md
@@ -0,0 +1,132 @@
+# Contributor Covenant Code of Conduct
+
+## Our Pledge
+
+We as members, contributors, and leaders pledge to make participation in our
+community a harassment-free experience for everyone, regardless of age, body
+size, visible or invisible disability, ethnicity, sex characteristics, gender
+identity and expression, level of experience, education, socio-economic status,
+nationality, personal appearance, race, caste, color, religion, or sexual
+identity and orientation.
+
+We pledge to act and interact in ways that contribute to an open, welcoming,
+diverse, inclusive, and healthy community.
+
+## Our Standards
+
+Examples of behavior that contributes to a positive environment for our
+community include:
+
+- Demonstrating empathy and kindness toward other people
+- Being respectful of differing opinions, viewpoints, and experiences
+- Giving and gracefully accepting constructive feedback
+- Accepting responsibility and apologizing to those affected by our mistakes,
+  and learning from the experience
+- Focusing on what is best not just for us as individuals, but for the overall
+  community
+
+Examples of unacceptable behavior include:
+
+- The use of sexualized language or imagery, and sexual attention or advances of
+  any kind
+- Trolling, insulting or derogatory comments, and personal or political attacks
+- Public or private harassment
+- Publishing others' private information, such as a physical or email address,
+  without their explicit permission
+- Other conduct which could reasonably be considered inappropriate in a
+  professional setting
+
+## Enforcement Responsibilities
+
+Community leaders are responsible for clarifying and enforcing our standards of
+acceptable behavior and will take appropriate and fair corrective action in
+response to any behavior that they deem inappropriate, threatening, offensive,
+or harmful.
+
+Community leaders have the right and responsibility to remove, edit, or reject
+comments, commits, code, wiki edits, issues, and other contributions that are
+not aligned to this Code of Conduct, and will communicate reasons for moderation
+decisions when appropriate.
+
+## Scope
+
+This Code of Conduct applies within all community spaces, and also applies when
+an individual is officially representing the community in public spaces.
+Examples of representing our community include using an official e-mail address,
+posting via an official social media account, or acting as an appointed
+representative at an online or offline event.
+
+## Enforcement
+
+Instances of abusive, harassing, or otherwise unacceptable behavior may be
+reported to the community leaders responsible for enforcement at
+<conduct@autoware.org>.
+All complaints will be reviewed and investigated promptly and fairly.
+
+All community leaders are obligated to respect the privacy and security of the
+reporter of any incident.
+
+## Enforcement Guidelines
+
+Community leaders will follow these Community Impact Guidelines in determining
+the consequences for any action they deem in violation of this Code of Conduct:
+
+### 1. Correction
+
+**Community Impact**: Use of inappropriate language or other behavior deemed
+unprofessional or unwelcome in the community.
+
+**Consequence**: A private, written warning from community leaders, providing
+clarity around the nature of the violation and an explanation of why the
+behavior was inappropriate. A public apology may be requested.
+
+### 2. Warning
+
+**Community Impact**: A violation through a single incident or series of
+actions.
+
+**Consequence**: A warning with consequences for continued behavior. No
+interaction with the people involved, including unsolicited interaction with
+those enforcing the Code of Conduct, for a specified period of time. This
+includes avoiding interactions in community spaces as well as external channels
+like social media. Violating these terms may lead to a temporary or permanent
+ban.
+
+### 3. Temporary Ban
+
+**Community Impact**: A serious violation of community standards, including
+sustained inappropriate behavior.
+
+**Consequence**: A temporary ban from any sort of interaction or public
+communication with the community for a specified period of time. No public or
+private interaction with the people involved, including unsolicited interaction
+with those enforcing the Code of Conduct, is allowed during this period.
+Violating these terms may lead to a permanent ban.
+
+### 4. Permanent Ban
+
+**Community Impact**: Demonstrating a pattern of violation of community
+standards, including sustained inappropriate behavior, harassment of an
+individual, or aggression toward or disparagement of classes of individuals.
+
+**Consequence**: A permanent ban from any sort of public interaction within the
+community.
+
+## Attribution
+
+This Code of Conduct is adapted from the [Contributor Covenant][homepage],
+version 2.1, available at
+[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1].
+
+Community Impact Guidelines were inspired by
+[Mozilla's code of conduct enforcement ladder][mozilla coc].
+
+For answers to common questions about this code of conduct, see the FAQ at
+[https://www.contributor-covenant.org/faq][faq]. Translations are available at
+[https://www.contributor-covenant.org/translations][translations].
+
+[homepage]: https://www.contributor-covenant.org
+[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html
+[mozilla coc]: https://github.com/mozilla/diversity
+[faq]: https://www.contributor-covenant.org/faq
+[translations]: https://www.contributor-covenant.org/translations
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 0000000..22c7ee2
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,3 @@
+# Contributing
+
+See <https://autowarefoundation.github.io/autoware-documentation/main/contributing/>.
diff --git a/CPPLINT.cfg b/CPPLINT.cfg
new file mode 100644
index 0000000..ba6bdf0
--- /dev/null
+++ b/CPPLINT.cfg
@@ -0,0 +1,14 @@
+# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_cpplint/ament_cpplint/main.py#L64-L120
+set noparent
+linelength=100
+includeorder=standardcfirst
+filter=-build/c++11               # we do allow C++11
+filter=-build/namespaces_literals # we allow using namespace for literals
+filter=-runtime/references        # we consider passing non-const references to be ok
+filter=-whitespace/braces         # we wrap open curly braces for namespaces, classes and functions
+filter=-whitespace/indent         # we don't indent keywords like public, protected and private with one space
+filter=-whitespace/parens         # we allow closing parenthesis to be on the next line
+filter=-whitespace/semicolon      # we allow the developer to decide about whitespace after a semicolon
+filter=-build/header_guard        # we automatically fix the names of header guards using pre-commit
+filter=-build/include_order       # we use the custom include order
+filter=-build/include_subdir      # we allow the style of "foo.hpp"
diff --git a/DISCLAIMER.md b/DISCLAIMER.md
new file mode 100644
index 0000000..1b5a9bb
--- /dev/null
+++ b/DISCLAIMER.md
@@ -0,0 +1,46 @@
+DISCLAIMER
+
+“Autoware” will be provided by The Autoware Foundation under the Apache License 2.0.
+This “DISCLAIMER” will be applied to all users of Autoware (a “User” or “Users”) with
+the Apache License 2.0 and Users shall hereby approve and acknowledge all the contents
+specified in this disclaimer below and will be deemed to consent to this
+disclaimer without any objection upon utilizing or downloading Autoware.
+
+Disclaimer and Waiver of Warranties
+
+1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND,
+   EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”)
+   including but not limited to any representation or warranty (i) of fitness or
+   suitability for a particular purpose contemplated by the Users, (ii) of the
+   expected functions, commercial value, accuracy, or usefulness of the Service,
+   (iii) that the use by the Users of the Service complies with the laws and
+   regulations applicable to the Users or any internal rules established by
+   industrial organizations, (iv) that the Service will be free of interruption or
+   defects, (v) of the non-infringement of any third party's right and (vi) the
+   accuracy of the content of the Services and the software itself.
+
+2. The Autoware Foundation shall not be liable for any damage incurred by the
+   User that are attributable to the Autoware Foundation for any reasons
+   whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR
+   INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS.
+
+3. A User shall be entirely responsible for the content posted by the User and
+   its use of any content of the Service or the Website. If the User is held
+   responsible in a civil action such as a claim for damages or even in a criminal
+   case, the Autoware Foundation and member companies, governments and academic &
+   non-profit organizations and their directors, officers, employees and agents
+   (collectively, the “Indemnified Parties”) shall be completely discharged from
+   any rights or assertions the User may have against the Indemnified Parties, or
+   from any legal action, litigation or similar procedures.
+
+Indemnity
+
+A User shall indemnify and hold the Indemnified Parties harmless from any of
+their damages, losses, liabilities, costs or expenses (including attorneys'
+fees or criminal compensation), or any claims or demands made against the
+Indemnified Parties by any third party, due to or arising out of, or in
+connection with utilizing Autoware (including the representations and
+warranties), the violation of applicable Product Liability Law of each country
+(including criminal case) or violation of any applicable laws by the Users, or
+the content posted by the User or its use of any content of the Service or the
+Website.
diff --git a/README.md b/README.md
deleted file mode 100644
index 8d1e47e..0000000
--- a/README.md
+++ /dev/null
@@ -1,2 +0,0 @@
-# autoware_lanelet2_extension
-Extension library of Lanelet2
diff --git a/build_depends.repos b/build_depends.repos
new file mode 100644
index 0000000..89e4b62
--- /dev/null
+++ b/build_depends.repos
@@ -0,0 +1,17 @@
+repositories:
+  autoware_auto_msgs:
+    type: git
+    url: https://github.com/tier4/autoware_auto_msgs.git
+    version: tier4/main
+  autoware_cmake:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_cmake.git
+    version: main
+  autoware_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_msgs.git
+    version: main
+  autoware_utils:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_utils.git
+    version: main
diff --git a/codecov.yaml b/codecov.yaml
new file mode 100644
index 0000000..8ca2196
--- /dev/null
+++ b/codecov.yaml
@@ -0,0 +1,22 @@
+coverage:
+  status:
+    project:
+      default:
+        target: auto
+    patch:
+      default:
+        target: auto
+
+comment:
+  show_carryforward_flags: true
+
+flag_management:
+  default_rules:
+    carryforward: true
+    statuses:
+      - name_prefix: project-
+        type: project
+        target: auto
+      - name_prefix: patch-
+        type: patch
+        target: auto
diff --git a/lanelet2_extension/CMakeLists.txt b/lanelet2_extension/CMakeLists.txt
new file mode 100644
index 0000000..f67e2bf
--- /dev/null
+++ b/lanelet2_extension/CMakeLists.txt
@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 3.14)
+project(lanelet2_extension)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(PkgConfig)
+find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
+  PATH_SUFFIXES GeographicLib
+)
+set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
+
+find_library(GeographicLib_LIBRARIES
+  NAMES Geographic
+)
+
+find_library(PUGIXML_LIBRARIES
+  NAMES pugixml
+)
+
+find_path(PUGIXML_INCLUDE_DIRS
+  NAMES pugixml.hpp
+  PATH_SUFFIXES pugixml
+)
+
+include_directories(
+  ${GeographicLib_INCLUDE_DIRS}
+  ${PUGIXML_INCLUDE_DIRS}
+)
+
+add_definitions(${GeographicLib_DEFINITIONS})
+
+ament_auto_add_library(lanelet2_extension_lib SHARED
+  lib/autoware_osm_parser.cpp
+  lib/autoware_traffic_light.cpp
+  lib/crosswalk.cpp
+  lib/detection_area.cpp
+  lib/landmark.cpp
+  lib/no_parking_area.cpp
+  lib/no_stopping_area.cpp
+  lib/message_conversion.cpp
+  lib/mgrs_projector.cpp
+  lib/query.cpp
+  lib/road_marking.cpp
+  lib/speed_bump.cpp
+  lib/transverse_mercator_projector.cpp
+  lib/utilities.cpp
+  lib/virtual_traffic_light.cpp
+  lib/visualization.cpp
+  lib/route_checker.cpp
+)
+target_link_libraries(lanelet2_extension_lib
+  ${GeographicLib_LIBRARIES}
+)
+get_target_property(lanelet2_core_INCLUDE_DIRECTORIES lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES)
+target_include_directories(lanelet2_extension_lib
+  SYSTEM PRIVATE
+    ${lanelet2_core_INCLUDE_DIRECTORIES}
+)
+
+ament_auto_add_executable(lanelet2_extension_sample src/sample_code.cpp)
+add_dependencies(lanelet2_extension_sample lanelet2_extension_lib)
+target_link_libraries(lanelet2_extension_sample
+  lanelet2_extension_lib
+)
+
+ament_auto_add_executable(autoware_lanelet2_validation src/validation.cpp)
+add_dependencies(autoware_lanelet2_validation lanelet2_extension_lib)
+target_link_libraries(autoware_lanelet2_validation
+  ${catkin_LIBRARIES}
+  ${PUGIXML_LIBRARIES}
+  lanelet2_extension_lib
+)
+
+ament_auto_add_executable(check_right_of_way src/check_right_of_way.cpp)
+add_dependencies(check_right_of_way lanelet2_extension_lib)
+target_link_libraries(check_right_of_way
+  ${catkin_LIBRARIES}
+  ${PUGIXML_LIBRARIES}
+  lanelet2_extension_lib
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(message_conversion-test test/src/test_message_conversion.cpp)
+  target_link_libraries(message_conversion-test lanelet2_extension_lib)
+  ament_add_ros_isolated_gtest(projector-test test/src/test_projector.cpp)
+  target_link_libraries(projector-test lanelet2_extension_lib)
+  ament_add_ros_isolated_gtest(query-test test/src/test_query.cpp)
+  target_link_libraries(query-test lanelet2_extension_lib)
+  ament_add_ros_isolated_gtest(regulatory_elements-test test/src/test_regulatory_elements.cpp)
+  target_link_libraries(regulatory_elements-test lanelet2_extension_lib)
+  ament_add_ros_isolated_gtest(utilities-test test/src/test_utilities.cpp)
+  target_link_libraries(utilities-test lanelet2_extension_lib)
+  target_include_directories(utilities-test
+  SYSTEM PRIVATE
+    ${lanelet2_core_INCLUDE_DIRECTORIES}
+  )
+  ament_add_ros_isolated_gtest(route-test test/src/test_route_checker.cpp)
+  target_link_libraries(route-test lanelet2_extension_lib)
+endif()
+
+ament_auto_package()
diff --git a/lanelet2_extension/README.md b/lanelet2_extension/README.md
new file mode 100644
index 0000000..85e360b
--- /dev/null
+++ b/lanelet2_extension/README.md
@@ -0,0 +1,101 @@
+# lanelet2_extension package
+
+This package contains external library for Lanelet2 and is meant to ease the use of Lanelet2 in Autoware.
+
+## Lanelet Format for Autoware
+
+Autoware uses extended Lanelet2 Format for Autoware, which means you need to add some tags to default OSM file if you want to fully use Lanelet2 maps. For details about custom tags, please refer to this [document](./docs/lanelet2_format_extension.md).
+
+## Code API
+
+### IO
+
+#### Autoware OSM Parser
+
+Autoware Lanelet2 Format uses .osm extension as original Lanelet2.
+However, there are some custom tags that is used by the parser.
+
+Currently, this includes:
+
+- reading `<MapMetaInfo>` tag which contains information about map format version and map version.
+
+The parser is registered as "autoware_osm_handler" as lanelet parser
+
+### Projection
+
+#### MGRS Projector
+
+MGRS projector projects latitude longitude into MGRS Coordinates.
+
+#### Transverse Mercator Projector
+
+This projector projects latitude and longitude into Transverse Mercator coordinate.
+This is similar to [UTM projector](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_projection/src/UTM.cpp) in the Lanelet2 library, but uses a custom central meridian on a given origin.
+
+### Regulatory Elements
+
+#### Autoware Traffic Light
+
+Autoware Traffic Light class allows you to retrieve information about traffic lights.
+Autoware Traffic Light class contains following members:
+
+- traffic light shape
+- light bulbs information of traffic lights
+- stopline associated to traffic light
+
+#### Right Of Way
+
+Autoware intersection module requires the information on which lanes can be ignored for object detection, like those lanes whose traffic light color is red when that of ego is green, etc. `right_of_way` tag is a regulatory element that consists of a tuple of (`right_of_way`s and `yield_lane`s), and `right_of_way`s have priority over`yield_lane`s. Although `right_of_way` tag itself is defined in Lanelet2, this package provides proper conditions on how this tag should be set.
+
+### Utility
+
+#### Message Conversion
+
+This contains functions to convert lanelet map objects into ROS messages.
+Currently it contains following conversions:
+
+- lanelet::LaneletMapPtr to/from autoware_auto_mapping_msgs::msg::HADMapBin
+- lanelet::Point3d to geometry_msgs::Point
+- lanelet::Point2d to geometry_msgs::Point
+- lanelet::BasicPoint3d to geometry_msgs::Point
+
+#### Query
+
+This module contains functions to retrieve various information from maps.
+e.g. crosswalks, trafficlights, stoplines
+
+#### Utilities
+
+This module contains other useful functions related to Lanelet.
+e.g. matching waypoint with lanelets
+
+#### Route Checker
+
+This module contains a function to check the loading route is valid or not.
+If it is invalid, puts warning without dying.
+The case valid is when the route is created on the same map as the current one.
+
+### Visualization
+
+Visualization contains functions to convert lanelet objects into visualization marker messages.
+Currently it contains following conversions:
+
+- lanelet::Lanelet to Triangle Markers
+- lanelet::LineString to LineStrip Markers
+- TrafficLights to Triangle Markers
+
+## Nodes
+
+### lanelet2_extension_sample
+
+Code for this explains how this lanelet2_extension library is used.
+The executable is not meant to do anything.
+
+### autoware_lanelet2_extension
+
+This node checks if an .osm file follows the Autoware version of Lanelet2 format.
+You can check by running:
+
+```sh
+ros2 run lanelet2_extension autoware_lanelet2_validation --ros-args -p map_file:=<path/to/map.osm>
+```
diff --git a/lanelet2_extension/docs/crosswalk_regulatory_element.svg b/lanelet2_extension/docs/crosswalk_regulatory_element.svg
new file mode 100644
index 0000000..794bff2
--- /dev/null
+++ b/lanelet2_extension/docs/crosswalk_regulatory_element.svg
@@ -0,0 +1,161 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="961px" height="381px" viewBox="-0.5 -0.5 961 381" style="background-color: rgb(255, 255, 255);">
+  <defs/>
+  <g>
+    <rect x="0" y="0" width="960" height="380" fill="#ffffff" stroke="none" pointer-events="all"/>
+    <rect x="40" y="140" width="880" height="80" fill="#b3b3b3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="40" y="220" width="880" height="80" fill="#b3b3b3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="635" y="115" width="90" height="210" fill-opacity="0.5" fill="#00ff00" stroke="rgb(0, 0, 0)" stroke-opacity="0.5" pointer-events="all"/>
+    <rect x="640" y="120" width="80" height="200" fill="#b3b3b3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="210" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="250" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="290" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="170" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="130" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="590" y="140" width="10" height="80" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="760" y="220" width="10" height="80" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="200" y="150" width="140" height="60" fill="#fff4c3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 128px; height: 1px; padding-top: 180px; margin-left: 212px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Type:lanelet
+                <br/>
+                SubType:road
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="212" y="184" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px">Type:lanelet...</text>
+      </switch>
+    </g>
+    <rect x="540" y="20" width="170" height="50" fill="#fff4c3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 158px; height: 1px; padding-top: 45px; margin-left: 552px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                Type:regulatory_element
+                <br/>
+                SubType:
+                <span>crosswalk</span>
+                <font color="#ff0000">
+                  <br/>
+                </font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="552" y="49" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" font-weight="bold">Type:regulatory_element...</text>
+      </switch>
+    </g>
+    <path d="M 660.79 123.68 L 667.5 70" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 660.14 128.89 L 657.53 121.51 L 660.79 123.68 L 664.48 122.38 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 576.33 71.58 L 270 150" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 581.42 70.28 L 575.5 75.4 L 576.33 71.58 L 573.77 68.62 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 625 70 L 597.51 134.15" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 595.44 138.97 L 594.98 131.16 L 597.51 134.15 L 601.41 133.92 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="627.5" y="325" width="105" height="30" fill="#66ff66" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 103px; height: 1px; padding-top: 340px; margin-left: 629px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Polygon
+                <br/>
+                crosswalk_polygon
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="680" y="344" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Polygon...</text>
+      </switch>
+    </g>
+    <rect x="540" y="165" width="85" height="30" fill="#66ff66" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 83px; height: 1px; padding-top: 180px; margin-left: 541px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Linestring3d
+                <br/>
+                ref_line
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="583" y="184" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Linestring3d...</text>
+      </switch>
+    </g>
+    <path d="M 761.53 214.66 L 667.5 70" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 764.39 219.06 L 757.64 215.1 L 761.53 214.66 L 763.51 211.29 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 634.35 161.17 L 625 70" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 634.89 166.39 L 630.69 159.78 L 634.35 161.17 L 637.65 159.07 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="650" y="205" width="60" height="30" fill="#66ff66" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 58px; height: 1px; padding-top: 220px; margin-left: 651px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Lanelet
+                <br/>
+                refers
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="680" y="224" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Lanelet...</text>
+      </switch>
+    </g>
+    <rect x="732.5" y="245" width="85" height="30" fill="#66ff66" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 83px; height: 1px; padding-top: 260px; margin-left: 734px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Linestring3d
+                <br/>
+                ref_line
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="775" y="264" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Linestring3d...</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.drawio.com/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>
diff --git a/lanelet2_extension/docs/crosswalk_traffic_light.svg b/lanelet2_extension/docs/crosswalk_traffic_light.svg
new file mode 100644
index 0000000..1c3dfd2
--- /dev/null
+++ b/lanelet2_extension/docs/crosswalk_traffic_light.svg
@@ -0,0 +1,121 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="960px" height="440px" viewBox="-0.5 -0.5 960 440" style="background-color: rgb(255, 255, 255);">
+  <defs/>
+  <g>
+    <rect x="0" y="0" width="960" height="440" fill="#ffffff" stroke="none" pointer-events="all"/>
+    <rect x="40" y="140" width="880" height="80" fill="#b3b3b3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="40" y="220" width="880" height="80" fill="#b3b3b3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="120" width="80" height="200" fill="#b3b3b3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="210" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="250" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="290" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="170" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="640" y="130" width="80" height="20" fill="#e6e6e6" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="590" y="140" width="10" height="80" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="760" y="220" width="10" height="80" fill="#ffffff" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="615" y="190" width="130" height="60" fill="#fff4c3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 118px; height: 1px; padding-top: 220px; margin-left: 627px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Type:lanelet
+                <br/>
+                SubType:crosswalk
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="627" y="224" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px">Type:lanelet...</text>
+      </switch>
+    </g>
+    <rect x="740" y="30" width="170" height="50" fill="#fff4c3" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 158px; height: 1px; padding-top: 55px; margin-left: 752px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;">
+              <div
+                style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; overflow-wrap: normal;"
+              >
+                Type:regulatory_element
+                <br/>
+                SubType:traffic_light
+                <font color="#ff0000">
+                  <br/>
+                </font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="752" y="59" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" font-weight="bold">Type:regulatory_element...</text>
+      </switch>
+    </g>
+    <path d="M 778.16 84.66 L 680 190" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 781.74 80.82 L 779.53 88.33 L 778.16 84.66 L 774.41 83.55 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="620" y="30" width="40" height="70" rx="6" ry="6" fill="#f1f4fa" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <rect x="630" y="40" width="20" height="20" fill="#00ff00" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <rect x="630" y="70" width="20" height="20" fill="#ff0000" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <rect x="700" y="340" width="40" height="70" rx="6" ry="6" fill="#f1f4fa" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <rect x="710" y="350" width="20" height="20" fill="#00ff00" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <rect x="710" y="380" width="20" height="20" fill="#ff0000" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <rect x="760" y="355" width="120" height="40" fill="#fff4c3" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 108px; height: 1px; padding-top: 375px; margin-left: 772px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: none; white-space: normal; overflow-wrap: normal;">
+                Linestring
+                <br/>
+                Type:traffic_light
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="772" y="379" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px">Linestring...</text>
+      </switch>
+    </g>
+    <rect x="490" y="45" width="120" height="40" fill="#fff4c3" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe flex-start; width: 108px; height: 1px; padding-top: 65px; margin-left: 502px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: left;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: none; white-space: normal; overflow-wrap: normal;">
+                Linestring
+                <br/>
+                Type:traffic_light
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="502" y="69" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px">Linestring...</text>
+      </switch>
+    </g>
+    <path d="M 740 55 L 616.35 64.51" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 611.11 64.91 L 617.83 60.89 L 616.35 64.51 L 618.36 67.87 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 825 80 L 820.12 348.63" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 820.02 353.88 L 816.65 346.82 L 820.12 348.63 L 823.65 346.95 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="none"/>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.drawio.com/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>
diff --git a/lanelet2_extension/docs/detection_area.png b/lanelet2_extension/docs/detection_area.png
new file mode 100644
index 0000000..0ae1087
Binary files /dev/null and b/lanelet2_extension/docs/detection_area.png differ
diff --git a/lanelet2_extension/docs/extra_lanelet_subtypes.md b/lanelet2_extension/docs/extra_lanelet_subtypes.md
new file mode 100644
index 0000000..b7b84eb
--- /dev/null
+++ b/lanelet2_extension/docs/extra_lanelet_subtypes.md
@@ -0,0 +1,45 @@
+# Extra Lanelet Subtypes
+
+## Roadside Lane
+
+The subtypes for this lanelet classify the outer lanes adjacent to the driving lane.Since the list of lanelet subtypes defined in this [link](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletAndAreaTagging.md) cannot represent the shoulder lane and pedestrian lane described below, two new subtypes are defined.When parking on the street, it is necessary to distinguish between a shoulder lane which can be used by vehicles, and a pedestrian lane which can be used by pedestrians and bicycles.If you park in a shoulder lane, you can use the entire lane for temporary parking, but if you park in a pedestrian lane, you must leave a space of at least 75cm.
+
+### Road shoulder subtype
+
+- refers: lanelet with subtype attribute. Subtype explains what the type of roadside it represents. If there is an area outside of this roadside lane that is open to traffic, such as a sidewalk or bike lane, select the road_shoulder subtype.
+
+![Road shoulder](road_shoulder.svg)
+
+Sample road shoulder in .osm format is shown below:
+
+```xml
+  <relation id="120700">
+    <member type="way" role="left" ref="34577"/>
+    <member type="way" role="right" ref="120694"/>
+    <tag k="type" v="lanelet"/>
+    <tag k="subtype" v="road_shoulder"/>
+    <tag k="speed_limit" v="10"/>
+    <tag k="location" v="urban"/>
+    <tag k="one_way" v="yes"/>
+  </relation>
+```
+
+### Pedestrian lane subtype
+
+- refers: lanelet with subtype attribute. Subtype explains what the type of roadside it represents. If there are no passable areas outside of this roadside lane, select the pedestrian_lane subtype.
+
+![Pedestrian lane](pedestrian_lane.svg)
+
+Sample pedestrian lane in .osm format is shown below:
+
+```xml
+  <relation id="120700">
+    <member type="way" role="left" ref="34577"/>
+    <member type="way" role="right" ref="120694"/>
+    <tag k="type" v="lanelet"/>
+    <tag k="subtype" v="pedestrian_lane"/>
+    <tag k="speed_limit" v="10"/>
+    <tag k="location" v="urban"/>
+    <tag k="one_way" v="yes"/>
+  </relation>
+```
diff --git a/lanelet2_extension/docs/extra_regulatory_elements.md b/lanelet2_extension/docs/extra_regulatory_elements.md
new file mode 100644
index 0000000..1debdf9
--- /dev/null
+++ b/lanelet2_extension/docs/extra_regulatory_elements.md
@@ -0,0 +1,149 @@
+# Extra Regulatory Elements
+
+## Detection Area
+
+This regulatory element specifies region of interest which vehicle must pay attention whenever it is driving along the associated lanelet. When there are any obstacle in the detection area, vehicle must stop at specified stopline.
+
+- refers: refers to detection area polygon. There could be multiple detection areas registered to a single regulatory element.
+- ref_line: refers to stop line of the detection area
+
+![Detection area](detection_area.png)
+
+Sample detection area in .osm format is shown below:
+
+```xml
+  <node id=1 version='1' lat='49.00541994701' lon='8.41565013855'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=2 version='1' lat='49.00542091657' lon='8.4156469497'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=3 version='1' lat='49.00542180052' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=4 version='1' lat='49.00541994701' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=5 version='1' lat='49.00542180052' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <node id=6 version='1' lat='49.00541994701' lon='8.41564400223'>
+    <tag k=’ele’ v=’0’/>
+  </node>
+  <way id=11 version='1'>
+    <nd ref=1 />
+    <nd ref=2 />
+    <nd ref=3 />
+    <nd ref=4 />
+    <nd ref=1 />
+    <tag k='type' v=’detection_area’ />
+    <tag k='area' v=’yes’ />
+  </way>
+  <way id=12 version="1">
+    <nd ref=5 />
+    <nd ref=6 />
+    <tag k='type' v=stop_line’ />
+  </way>
+  <relation id="13">
+     <tag k="type" v="regulatory_element"/>
+     <tag k="subtype" v="detection_area"/>
+     <member type="way" ref="11" role="refers"/>
+     <member type="way" ref="12" role="ref_line"/>
+   </relation>
+```
+
+## Road Marking
+
+This regulatory element specifies related road markings to a lanelet as shown below.
+
+\* Note that the stopline in the image is for stoplines that are for reference, and normal stoplines should be expressed using TrafficSign regulatory element.
+
+refers: linestring with type attribute. Type explains what road marking it represents (e.g. stopline).
+
+![Road marking](road_mark.png)
+
+## Speed Bump
+
+This regulatory element specifies a speed bump on the road.
+
+- refers: to a speed bump polygon. There must be only one speed bump polygon registered to a single
+  regulatory element.
+
+![Speed bump](speed_bump.png)
+
+An example annotation of speed_bump:
+
+```xml
+  <node id='1' lat='40.81478464668' lon='29.35984444811'>
+    <tag k='ele' v='3.004897' />
+    <tag k='x' v='138.2702' />
+    <tag k='y' v='326.5387' />
+  </node>
+  <node id='2' lat='40.81476705154' lon='29.3599302694'>
+    <tag k='ele' v='2.949194' />
+    <tag k='x' v='145.5616' />
+    <tag k='y' v='324.7803' />
+  </node>
+  <node id='3' lat='40.81477166944' lon='29.35993194259'>
+    <tag k='ele' v='2.948368' />
+    <tag k='x' v='145.6889' />
+    <tag k='y' v='325.2968' />
+  </node>
+  <node id='4' lat='40.81478914798' lon='29.35984590876'>
+    <tag k='ele' v='3.018582' />
+    <tag k='x' v='138.3799' />
+    <tag k='y' v='327.0417' />
+  </node>
+  <way id='5'>
+    <nd ref='1' />
+    <nd ref='2' />
+    <nd ref='3' />
+    <nd ref='4' />
+    <tag k='area' v='yes' />
+    <tag k='height' v='0.15' />
+    <tag k='type' v='speed_bump' />
+  </way>
+  <relation id='6'>
+    <member type='way' ref='5' role='refers' />
+    <tag k='subtype' v='speed_bump' />
+    <tag k='type' v='regulatory_element' />
+  </relation>
+  <relation id='7'>
+    <member type='relation' ref='6' role='regulatory_element' />
+    <member type='way' ref='10' role='left' />
+    <member type='way' ref='11' role='right' />
+    <tag k='location' v='urban' />
+    <tag k='participant:vehicle' v='yes' />
+    <tag k='speed_limit' v='30.00' />
+    <tag k='subtype' v='road' />
+    <tag k='type' v='lanelet' />
+  </relation>
+  <relation id='8'>
+    <member type='relation' ref='6' role='regulatory_element' />
+    <member type='way' ref='12' role='left' />
+    <member type='way' ref='10' role='right' />
+    <tag k='location' v='urban' />
+    <tag k='participant:vehicle' v='yes' />
+    <tag k='speed_limit' v='30.00' />
+    <tag k='subtype' v='road' />
+    <tag k='type' v='lanelet' />
+  </relation>
+```
+
+- As an option `slow_down_speed` tag can be added to the speed bump way annotation to override the
+  speed calculated in planning module wrt the speed bump `height`. The value specified in
+  `slow_down_speed` tag must be in **kph**. So if this option is used then the way annotation should
+  look like as below:
+
+```xml
+  <way id='5'>
+    <nd ref='1' />
+    <nd ref='2' />
+    <nd ref='3' />
+    <nd ref='4' />
+    <tag k='area' v='yes' />
+    <tag k='height' v='0.15' />
+    <tag k='slow_down_speed' v='7.0' />
+    <tag k='type' v='speed_bump' />
+  </way>
+```
diff --git a/lanelet2_extension/docs/lanelet2_format_extension.md b/lanelet2_extension/docs/lanelet2_format_extension.md
new file mode 100644
index 0000000..744fd12
--- /dev/null
+++ b/lanelet2_extension/docs/lanelet2_format_extension.md
@@ -0,0 +1,481 @@
+# Modifying Lanelet2 format for Autoware
+
+## Overview
+
+About the basics of the default format, please refer to main [Lanelet2 repository](https://github.com/fzi-forschungszentrum-informatik/Lanelet2). (see [here](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletPrimitives.md) about primitives)
+
+In addition to default Lanelet2 Format, users should add following mandatory/optional tags to their osm lanelet files as explained in reset of this document.
+Users may use `autoware_lanelet2_validation` [node](../README.md#nodes) to check if their maps are valid.
+
+The following is the extra format added for Autoware:
+
+- [extra regulatory elements](extra_regulatory_elements.md)
+  - Detection Area
+  - Road Marking
+- [extra lanelet subtype](extra_lanelet_subtypes.md)
+  - Roadside Lane
+
+## Mandatory Tags
+
+### Elevation Tags
+
+Elevation("ele") information for points(`node`) is optional in default Lanelet2 format.
+However, some of Autoware packages(e.g. trafficlight_recognizer) need elevation to be included in HD map. Therefore, users must make sure that all points in their osm maps contain elevation tags.
+
+Here is an example osm syntax for node object.
+
+```xml
+<node id='1' visible='true' version='1' lat='49.00501435943' lon='8.41687458512'>
+  <tag k='ele' v='3.0'/> <!-- this tag is mandatory for Autoware!! -->
+</node>
+```
+
+### TrafficLights
+
+Default Lanelet2 format uses LineString(`way`) or Polygon class to represent the shape of a traffic light. For Autoware, traffic light objects must be represented only by LineString to avoid confusion, where start point is at bottom left edge and end point is at bottom right edge. Also, "height" tag must be added in order to represent the size in vertical direction (not the position).
+
+The Following image illustrates how LineString is used to represent shape of Traffic Light in Autoware.
+![How LineString is used to represent shape of Traffic Light in Autoware](traffic_light.png)
+
+Here is an example osm syntax for traffic light object.
+
+```xml
+<way id='13' visible='true' version='1'>
+  <nd ref='6' />
+  <nd ref='5' />
+  <tag k='type' v='traffic_light' />
+  <tag k='subtype' v='red_yellow_green' />
+  <tag k='height' v='0.5'/> <!-- this tag is mandatory for Autoware!! -->
+</way>
+```
+
+### Turn Directions
+
+Users must add "turn_direction" tags to lanelets within intersections to indicate vehicle's turning direction. You do not need this tags for lanelets that are not in intersections. If you do not have this tag, Autoware will not be able to light up turning indicators.
+This tags only take following values:
+
+- left
+- right
+- straight
+
+Following image illustrates how lanelets should be tagged.
+
+![Turn Directions: How lanelets should be tagged](turn_direction.png)
+
+Here is an example of osm syntax for lanelets in intersections.
+
+```xml
+<relation id='1' visible='true' version='1'>
+  <member type='way' ref='2' role='left' />
+  <member type='way' ref='3' role='right' />
+  <member type='relation' ref='4' role='regulatory_element' />
+  <tag k='location' v='urban' />
+  <tag k='one_way' v='yes' />
+  <tag k='subtype' v='road' />
+  <tag k='type' v='lanelet' />
+  <tag k='turn_direction' v='left' /> <!-- this tag is mandatory for lanelets at intersections!! -->
+</relation>
+```
+
+### Right Of Way
+
+Users must add `right_of_way` tag to intersection lanes, namely lanes with `turn_direction` attribute. Below image illustrates how to set yield lanes(orange) for the ego lane(blue).
+
+![RightOfWay tagging](right_of_way.drawio.svg)
+
+Basically intersection lanes which are:
+
+- left/right turn
+- straight and on the side of priority sign
+
+need this tag to know which lanes in their `conflicting lanes` can be ignored for object detection.
+
+left/right turning lane is often conflicting with lanes whose traffic lights are red when its traffic light is green, so **at least** those lanes should be registered as yield lanes.
+
+If ego car is going straight the intersection when the traffic light is green, then it does not need to care other lanes because it has the highest priority. But if the traffic lights do not exist and ego lane is on the side of priority road, then yield lanes should be set to explicitly ignore part of conflicting lanes.
+
+## Optional Taggings
+
+Following tags are optional tags that you may want to add depending on how you want to use your map in Autoware.
+
+### Meta Info
+
+Users may add the `MetaInfo` element to their OSM file to indicate format version and map version of their OSM file. This information is not meant to influence Autoware vehicle's behavior, but is published as ROS message so that developers could know which map was used from ROSBAG log files. MetaInfo elements exists in the same hierarchy with `node`, `way`, and `relation` elements, otherwise JOSM wouldn't be able to load the file correctly.
+
+Here is an example of MetaInfo in osm file:
+
+```xml
+<?xml version='1.0' encoding='UTF-8'?>
+<osm version='0.6' generator='JOSM'>
+  <MetaInfo format_version='1.0' map_version='1.0'/>
+  <node>...</node>
+  <way>...</way>
+  <relation>...</relation>
+</osm>
+```
+
+### Local Coordinate Expression
+
+Sometimes users might want to create Lanelet2 maps that are not georeferenced.
+In such a case, users may use "local_x", "local_y" taggings to express local positions instead of latitude and longitude.
+You will need to `lanelet2_map_projector_type` to `local`, then [autoware map loader](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_loader#lanelet2_map_loader) will overwrite x,y positions with these tags when they are present.
+For z values, use "ele" tags as default Lanelet2 Format.
+You would still need to fill in lat and lon attributes so that parser does not crush, but their values could be anything.
+
+Here is example `node` element in osm with "local_x", "local_y" taggings:
+
+```xml
+<!-- lat/lon attributes are required, but their values can be anything -->
+<node id='40648' visible='true' version='1' lat='0' lon='0'>
+  <tag k='local_x' v=2.54'/>
+  <tag k='local_y' v=4.38'/>
+  <tag k='ele' v='3.0'/>
+</node>
+```
+
+### Light Bulbs in Traffic Lights
+
+Default Lanelet format can only express shape (base + height) of traffic lights.
+However, region_tlr node in Autoware uses positions of each light bulbs to recognize color of traffic light. If users may wish to use this node, "light_bulbs"(`way`) element must be registered to traffic_light regulatory_element object define position and color of each light bulb in traffic lights. If you are using other trafficlight_recognizer nodes(e.g. tlr_mxnet), which only uses bounding box of traffic light, then you do not need to add this object.
+
+"light_bulbs" object is defined using LineString(`way`), and each node of line string is placed at the center of each light bulb. Also, each node should have "color" and optionally "arrow" tags to describe its type. Also, "traffic_light_id" tag is used to indicate which ID of relevant traffic_light element.
+
+"color" tag is used to express the color of the light bulb. Currently only three values are used:
+
+- red
+- yellow
+- green
+
+"arrow" tag is used to express the direction of the arrow of light bulb:
+
+- up
+- right
+- left
+- up_right
+- up_left
+
+Following image illustrates how "light_bulbs" LineString should be created.
+
+![How "light_bulbs" LineString should be created](light_bulbs.png)
+
+Here is an example of osm syntax for light_bulb object:
+
+```xml
+<node id=1 version='1' lat='49.00541994701' lon='8.41565013855'>
+  <tag k='ele' v='5'/>
+  <tag k='color' v='red'/>
+</node>
+<node id=2 version='1' lat='49.00542091657' lon='8.4156469497'>
+  <tag k='ele' v='5'/>
+  <tag k='color' v='yellow'/>
+</node>
+<node id=3 version='1' lat='49.00542180052' lon='8.41564400223'>
+  <tag k='ele' v='5'/>
+  <tag k='color' v='green'/>
+</node>
+<node id=3 version='1' lat='49.00542180052' lon='8.41564400223'>
+  <tag k='ele' v='4.6'/>
+  <tag k='color' v='green'/>
+  <tag k=arrow v='right'/>
+</node>
+<way id=11 version='1'>
+  <nd ref=1 />
+  <nd ref=2 />
+  <nd ref=3 />
+  <tag k='traffic_light_id' v='10'/> <!-- id of linestring with type="traffic_light" -->
+  <tag k='type' v='light_bulbs' />
+</way>
+```
+
+After creating "light_bulbs" elements, you have to register them to traffic_light regulatory element as role "light_bulbs".
+The following illustrates how light_bulbs are registered to traffic_light regulatory elements.
+
+![How light_bulbs are registered to traffic_light regulatory elements](traffic_light_regulatory_element.png)
+
+```xml
+<relation id='8' visible='true' version='1'>
+  <tag k='type' v='regulatory_element' />
+  <tag k='subtype' v='traffic_light' />
+  <member type='way' ref='9' role='ref_line' />
+  <member type='way' ref='10' role='refers' /> <!-- refers to the traffic light line string -->
+  <member type='way' ref='11' role='light_bulbs'/> <!-- refers to the light_bulb line string -->
+</relation>
+```
+
+### Crosswalk
+
+Original Lanelet2 format only requires `subtype=crosswalk` tag to be specified in the corresponding lanelet. However, Autoware requires a regulatory element to be defined on top of that in order to:
+
+- explicitly define the relevant driving lanes even in 3D environment
+- optionally define stop lines associated with the crosswalk
+- enable accurate definition of complex polygons for crosswalk
+
+For the details, refer to this [GitHub discussion](https://github.com/orgs/autowarefoundation/discussions/3036).
+Crosswalk regulatory element can be tied to `ref_line`, `crosswalk_polygon` and `refers`.
+
+![crosswalk_regulatory elements](crosswalk_regulatory_element.svg)
+
+- `ref_line`: Stop line for the crosswalk.
+- `crosswalk_polygon`: Accurate area of the crosswalk.
+- `refers`: Lanelet that indicates the moving direction of crosswalk users.
+
+_An example:_
+
+```xml
+<relation id="10751">
+  <member type="way" role="ref_line" ref="8123"/>
+  <member type="way" role="crosswalk_polygon" ref="4047"/>
+  <member type="relation" role="refers" ref="2206"/>
+  <tag k="type" v="regulatory_element"/>
+  <tag k="subtype" v="crosswalk"/>
+</relation>
+```
+
+### Traffic Lights for Crosswalks
+
+It can define not only traffic lights for vehicles but also for crosswalk users by using regulatory element. In this case, the crosswalk lanelet needs to refer the traffic light regulatory element.
+
+![crosswalk_traffic_light](crosswalk_traffic_light.svg)
+
+_An example:_
+
+```xml
+<way id="179745">
+  <nd ref="179743"/>
+  <nd ref="179744"/>
+  <tag k="type" v="traffic_light"/>
+  <tag k="subtype" v="solid"/>
+  <tag k="height" v="0.5"/>
+</way>
+
+...
+
+<relation id="179750">
+  <member type="way" role="refers" ref="179745"/>
+  <member type="way" role="refers" ref="179756"/>
+  <tag k="type" v="regulatory_element"/>
+  <tag k="subtype" v="traffic_light"/>
+</relation>
+
+...
+
+<relation id="1556">
+  <member type="way" role="left" ref="1449"/>
+  <member type="way" role="right" ref="1450"/>
+  <member type="relation" role="regulatory_element" ref="179750"/>
+  <tag k="type" v="lanelet"/>
+  <tag k="subtype" v="crosswalk"/>
+  <tag k="speed_limit" v="10"/>
+  <tag k="location" v="urban"/>
+  <tag k="one_way" v="no"/>
+  <tag k="participant:pedestrian" v="yes"/>
+</relation>
+```
+
+### Safety Slow Down for Crosswalks
+
+If you wish ego vehicle to slow down to a certain speed from a certain distance while passing over a
+certain crosswalk _even though there are no target objects around it_, you can add following tags to
+the crosswalk definition on lanelet2 map:
+
+- `safety_slow_down_speed` **[m/s]**: The speed you want ego vehicle to drive at while passing over
+  the crosswalk
+- `safety_slow_down_distance` **[m]**: The distance between front bumper of ego vehicle and
+  closest point to the crosswalk when ego vehicle slows down and drives at specified speed
+
+_An example:_
+
+```xml
+<relation id='34378' visible='true' version='1'>
+  <member type='way' ref='34374' role='left' />
+  <member type='way' ref='34377' role='right' />
+  <tag k='subtype' v='crosswalk' />
+  <tag k='safety_slow_down_speed' v='3.0' />
+  <tag k='safety_slow_down_distance' v='2.0' />
+  <tag k='type' v='lanelet' />
+</relation>
+```
+
+### No Obstacle Segmentation Area
+
+If there is a polygon area that has `no_obstacle_segmentation_area` tag, the obstacle points in this area are removed.
+If you want to ignore points for a certain module, you have to define another tag and specify it in the parameter of vector_map_inside_area_filter.
+Currently, following tags are defined other than `no_obstacle_segmentation_area`.
+
+- `no_obstacle_segmentation_area_for_run_out`
+  - remove points for run out module
+
+_An example:_
+
+```xml
+  <way id="1658">
+    <nd ref="1653"/>
+    <nd ref="1654"/>
+    <nd ref="1656"/>
+    <nd ref="1657"/>
+    <tag k="type" v="no_obstacle_segmentation_area"/>
+    <tag k="area" v="yes"/>
+  </way>
+```
+
+### Hatched Road Markings Area
+
+The area with `hatched_road_markings` tag can be used for avoiding obstacles when there is not enough space to avoid.
+Note that in some countries, it is forbidden for vehicles to go inside the area.
+
+_An example:_
+
+```xml
+  <way id="9933">
+    <nd ref="2058"/>
+    <nd ref="2059"/>
+    <nd ref="1581"/>
+    <nd ref="2057"/>
+    <nd ref="4925"/>
+    <nd ref="4923"/>
+    <nd ref="4921"/>
+    <nd ref="4920"/>
+    <tag k="type" v="hatched_road_markings"/>
+    <tag k="area" v="yes"/>
+  </way>
+```
+
+### No Stopping Area
+
+The area with `no_stopping_area` tag can be used to prohibit even a few seconds of stopping, even for traffic jams or at traffic lights.
+The ref_line can be set arbitrarily, and the ego-vehicle should stop at this line if it cannot pass through the area.
+
+_An example:_
+
+```xml
+  <way id='9853' visible='true' version='1'>
+    <nd ref='9849' />
+    <nd ref='9850' />
+    <nd ref='9851' />
+    <nd ref='9852' />
+    <tag k='area' v='yes' />
+    <tag k='type' v='no_stopping_area' />
+  </way>
+
+  <relation id='9854' visible='true' version='1'>
+    <member type='way' ref='9853' role='refers' />
+    <member type='way' ref='9848' role='ref_line' />
+    <tag k='subtype' v='no_stopping_area' />
+    <tag k='type' v='regulatory_element' />
+  </relation>
+```
+
+### No Parking Area
+
+The area with `no_parking_area` tag can be used to prohibit parking. Stopping for a few seconds is allowed in this area.
+
+_An example:_
+
+```xml
+  <way id='9853' visible='true' version='1'>
+    <nd ref='9849' />
+    <nd ref='9850' />
+    <nd ref='9851' />
+    <nd ref='9852' />
+    <tag k='area' v='yes' />
+    <tag k='type' v='no_parking_area' />
+  </way>
+
+  <relation id='9854' visible='true' version='1'>
+    <member type='way' ref='9853' role='refers' />
+    <tag k='subtype' v='no_parking_area' />
+    <tag k='type' v='regulatory_element' />
+  </relation>
+```
+
+### No Drivable Lane
+
+A no drivable lane is a lanelet or more that are out of operation design domain (ODD), i.e., the vehicle **must not** drive autonomously in this/these lanelet/s.  
+A lanelet becomes no drivable by adding an optional tag under the relevant lanelet in the map file `<tag k="no_drivable_lane" v="yes"/>`.
+
+_An example:_
+
+```xml
+<relation id="2621">
+    <member type="way" role="left" ref="2593"/>
+    <member type="way" role="right" ref="2620"/>
+    <tag k="type" v="lanelet"/>
+    <tag k="subtype" v="road"/>
+    <tag k="speed_limit" v="30.00"/>
+    <tag k="location" v="urban"/>
+    <tag k="one_way" v="yes"/>
+    <tag k="participant:vehicle" v="yes"/>
+    <tag k="no_drivable_lane" v="yes"/>
+  </relation>
+```
+
+For more details about the `no_drivable_lane` concept and design, please refer to the [**_no-drivable-lane-design_**](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_velocity_no_drivable_lane_module/README.md) document.
+
+### Localization Landmarks
+
+Landmarks, such as AR-Tags, can be defined into the lanelet map to aid localization module.
+
+Landmark Specifications:
+
+<!-- cspell:ignore apiltag -->
+
+- **Shape**: Landmarks must be flat and defined as a polygon with exactly four vertices.
+- **Vertex Definition**: Vertices must be defined in a counter-clockwise order.
+- **Coordinate System** and Vertex Order:
+  - The x-axis must be parallel to the vector extending from the first vertex to the second vertex.
+  - The y-axis must be parallel to the vector extending from the second vertex to the third vertex.
+- **Required Attributes**:
+  - Specify pose_marker for type `<tag k="type" v="pose_marker"/>`
+  - Specify the type of the landmark for subtype `<tag k="subtype" v="apriltag_16h5"/>`
+  - Specify the ID of the landmark for marker_id `<tag k="marker_id" v="0"/>`
+    - ID can be assigned arbitrarily as a string, but the same ID must be assigned for the same marker type
+    - For example, apiltag_16h5 has 30 different IDs from 0 to 29. If multiple tags of the same type are to be placed in one environment, they should be assigned the same ID.
+
+![localization_landmark](./localization_landmark.drawio.svg)
+
+_An example:_
+
+```xml
+...
+
+  <node id="1" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="22.2356"/>
+    <tag k="local_y" v="87.4506"/>
+    <tag k="ele" v="2.1725"/>
+  </node>
+  <node id="2" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="22.639"/>
+    <tag k="local_y" v="87.5886"/>
+    <tag k="ele" v="2.5947"/>
+  </node>
+  <node id="3" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="22.2331"/>
+    <tag k="local_y" v="87.4713"/>
+    <tag k="ele" v="3.0208"/>
+  </node>
+  <node id="4" lat="35.8xxxxx" lon="139.6xxxxx">
+    <tag k="mgrs_code" v="99XXX000000"/>
+    <tag k="local_x" v="21.8298"/>
+    <tag k="local_y" v="87.3332"/>
+    <tag k="ele" v="2.5985"/>
+  </node>
+
+...
+
+  <way id="5">
+    <nd ref="1"/>
+    <nd ref="2"/>
+    <nd ref="3"/>
+    <nd ref="4"/>
+    <tag k="type" v="pose_marker"/>
+    <tag k="subtype" v="apriltag_16h5"/>
+    <tag k="area" v="yes"/>
+    <tag k="marker_id" v="0"/>
+  </way>
+
+...
+
+```
diff --git a/lanelet2_extension/docs/light_bulbs.png b/lanelet2_extension/docs/light_bulbs.png
new file mode 100644
index 0000000..dc1e6e1
Binary files /dev/null and b/lanelet2_extension/docs/light_bulbs.png differ
diff --git a/lanelet2_extension/docs/localization_landmark.drawio.svg b/lanelet2_extension/docs/localization_landmark.drawio.svg
new file mode 100644
index 0000000..5895d8e
--- /dev/null
+++ b/lanelet2_extension/docs/localization_landmark.drawio.svg
@@ -0,0 +1,156 @@
+<svg
+  host="65bd71144e"
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="336px"
+  height="336px"
+  viewBox="-0.5 -0.5 336 336"
+  content="&lt;mxfile&gt;&lt;diagram id=&quot;z_GJx55S16-dNHi1224P&quot; name=&quot;ページ1&quot;&gt;5ZhNb6MwEIZ/DdLuDWwC9Nhm2+5hV1ophz1beAJWDY6Mk5D++jXFfNmJkgNNqk0OEbz+wPPMeDzg4WVRv0qyyX8LCtxDPq09/MNDCCeJ/m+EQytEKGyFTDLaSsEgrNg7GNE36pZRqCYdlRBcsc1UTEVZQqomGpFS7Kfd1oJPn7ohGTjCKiXcVf8yqvJWTVA86D+BZXn35CB6aFsK0nU2llQ5oWI/kvCzh5dSCNVeFfUSeMOu49KOeznR2i9MQqkuGRAu2hE7wrfGOLMwdeislWJbUmgG+B5+2udMwWpD0qZ1r92rtVwVXN8F+tJdgFnTDqSCeiSZBb2CKEDJg+7StUYGjomOoIO1H1j3ffIx504kxr9ZP/eAQF8YCieIRA6RwEGih+hYg/M4SLVpA3DN6gbhLHzCKR+UuHzQETxoDjqxQ8cNmJvSweiGdBKHDv7adAL/inQeHDrh16Jj76xr0ln45zMxlPSxOb70XcpJVbH0wuQLdHKguQBGBi6OGNhpEjhRbDfNi8esNk/4I5heSc83sbamb3GrxFamYAaNTy1rntA/M5EiMgPlTPThg97qy9wSOG6pHb/o8FJTT1RKijdYCi6kVkpRNhG9ZpxbEuEsKxt3ao+B1p+aYGW60ng0DQWjlJ/aDtOTeS1KZWqlIJopXcTWhojdDYGPxAueY0Mgh/zhfsj3te6hC/nrgcf/fyZCVqbHdul4aSqyJwo/LxOFF7jllqcnsmuLK1ZeC7dqnxHOOH14eo99/OaCFlvQYgda5DKLZkDmlvLv95NfgweLe4Svl2Dd14RfpKQFkW/fXqS29fv9+CE5e8wFwTxu0LfDd5U2/w4fp/DzPw==&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <rect x="87" y="87" width="160" height="160" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="77" cy="257" rx="10" ry="10" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 18px; height: 1px; padding-top: 257px; margin-left: 68px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                1
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="77" y="261" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">1</text>
+      </switch>
+    </g>
+    <ellipse cx="257" cy="257" rx="10" ry="10" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 18px; height: 1px; padding-top: 257px; margin-left: 248px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                2
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="257" y="261" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">2</text>
+      </switch>
+    </g>
+    <ellipse cx="257" cy="77" rx="10" ry="10" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 18px; height: 1px; padding-top: 77px; margin-left: 248px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                3
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="257" y="81" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">3</text>
+      </switch>
+    </g>
+    <ellipse cx="77" cy="77" rx="10" ry="10" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 18px; height: 1px; padding-top: 77px; margin-left: 68px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                4
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="77" y="81" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">4</text>
+      </switch>
+    </g>
+    <path d="M 7 167 L 320.63 167" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 325.88 167 L 318.88 170.5 L 320.63 167 L 318.88 163.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="297" y="137" width="30" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 28px; height: 1px; padding-top: 152px; margin-left: 298px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                x
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="312" y="157" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">x</text>
+      </switch>
+    </g>
+    <rect x="137" y="7" width="30" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 28px; height: 1px; padding-top: 22px; margin-left: 138px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                y
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="152" y="27" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">y</text>
+      </switch>
+    </g>
+    <path d="M 167 327 L 167 13.37" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 167 8.12 L 170.5 15.12 L 167 13.37 L 163.5 15.12 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <ellipse cx="157" cy="157" rx="10" ry="10" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <ellipse cx="157" cy="157" rx="3" ry="3" fill="#000000" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <rect x="124" y="130" width="30" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 28px; height: 1px; padding-top: 145px; margin-left: 125px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                z
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="139" y="150" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">z</text>
+      </switch>
+    </g>
+    <rect x="7" y="7" width="110" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 108px; height: 1px; padding-top: 22px; margin-left: 8px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 16px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                Landmark(Front)
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="62" y="27" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="16px" text-anchor="middle">Landmark(Front)</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>
diff --git a/lanelet2_extension/docs/pedestrian_lane.svg b/lanelet2_extension/docs/pedestrian_lane.svg
new file mode 100644
index 0000000..05ec92f
--- /dev/null
+++ b/lanelet2_extension/docs/pedestrian_lane.svg
@@ -0,0 +1,90 @@
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="484px"
+  height="403px"
+  viewBox="-0.5 -0.5 484 403"
+  content="&lt;mxfile&gt;&lt;diagram id=&quot;k8MIM5X1AGlBXXJi1RH8&quot; name=&quot;Page-1&quot;&gt;5Vjfb9owEP5rkNqHIRIDgcf+3sukStW09WlykyOxZuLMMQX21+8c2yROYCAG07rSqvV9sc++u88fdnrkZr56kLTIPokEeC8cJKseue2F4Tia4l8NrA0QhENikFSyxGI18MR+ggUHFl2wBEqvoxKCK1b4YCzyHGLlYVRKsfS7zQT3Zy1oCh3gKaa8i35hicoMOgmjGv8ILM3czMHYRjynrrONpMxoIpYNiNz1yI0UQpnWfHUDXCfP5cWMu9/xdLMwCbk6aEBoRrxSvrDB2YWptYt2mTEFTwWNtb3EivbIdabmHK0Am7QsTI5nbAXo9nrGOL8RXMhqOBlXH8RLJcV3aDwZ3uoffNJdtQ3kFaSCVQOyUTyAmIOSa+zink4thSynpjbBy7o+ZGixrFGbDUgtJ9KN6zpt2LCZ25FFsj+LkCdXmnpo5SIHP4cmNY5LBBHMhFx/RWPQD0fOfta2M25XnrV21oqp5jg0n+0sul2P0oYbtLMApVjIGHyuKCpTUD4GibdjumVq1GG0pQwOk8CpYq/+PttWGjvDo2C44poFE58FZNCqronHjmrui5ajkPiONgt0jkwSOo4qpmzCPow8w3OSJzqSPNH7Iw9+vfRHXtWHwbH06bgK2kw8IYFGxxAooWWm5boq7qFsOpJM749LJGoJ0eRIJg3DliNyPh6N9/NIikWeVLTR1dpzMtgcbnTf8juoOLNGymlZugfescCyU6BTpnTMZNA9Ukyn9/eEnObggCXfd3AYBec6N0T/67nhH9mG7eIO27vnYEHfwZIz7MLJFk6MudKSzV49box/LPQ94RqZrj5QztK8R66wB4eZqp9iK7X/Ky9lQfOtbmKzwbQHmb5cYIT4i+sdNFqXXb9qXYCdl+bAQbmJMFIzlz8/wlUgPjoTVbrjzSavp8FDnP7sjuiUeTFYuXipwyoAL5tKMpp/0xEeFR6iJkIHt3Y5SqWe7hbvmFws1H5tLUAyZBXIetBjDV37Ou0Jsae9TVHefT/bJb+ppAnDrd8S70KUTDGRh5WcBNMTCTUZ+9+EXaEOtt3wolMI9fScX41bKxJsy7v+/L378rmuy5vVvhWJ+5zTOIayZC/oT79CAnqUCpwmumOk8gL6aR8HzSDHQEykBaqZsm0lwcHIv/7lTt0y8IvcF/Cb0reDdGwS6cBpqddLqvdZ5r1kdUB1Xc11Jzrv3nWHo98Iobt+/KEQolm/izSHmfqNLrn7BQ==&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <rect x="143" y="60" width="340" height="340" fill="#666666" stroke="#4d4d4d" pointer-events="all"/>
+    <path d="M 228 400 L 228 60" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 398 400 L 398 60" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 313 400 L 313 60" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" stroke-dasharray="9 9" pointer-events="stroke"/>
+    <rect x="175" y="60" width="51" height="340" fill-opacity="0.3" fill="#99ff33" stroke="none" pointer-events="all"/>
+    <path d="M 175 400 L 175 60" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 186 0 L 326 0 L 326 40 L 276 40 L 212.6 70 L 256 40 L 186 40 Z" fill="#99ff33" stroke="#4d4d4d" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 20px; margin-left: 187px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">
+                <div style="text-align: left">
+                  <span style="color: rgb(0 , 0 , 0)">type: lanelet</span>
+                </div>
+                <font color="#000000">
+                  <div style="text-align: left">
+                    <span>subtype: pedestrian_lane</span>
+                  </div>
+                </font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="256" y="24" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">type: lanelet subtype: p...</text>
+      </switch>
+    </g>
+    <rect x="143" y="60" width="30" height="340" fill="none" stroke="none" pointer-events="all"/>
+    <path
+      d="M 142.96 60.04 C 142.96 60.04 142.96 60.04 142.96 60.04 M 142.96 60.04 C 142.96 60.04 142.96 60.04 142.96 60.04 M 142.7 66.44 C 144.81 64.09 148.05 61.19 148.61 59.65 M 142.7 66.44 C 144.7 64.8 145.85 63.14 148.61 59.65 M 143.1 72.08 C 147.07 68.74 149.48 65.85 153.59 60.01 M 143.1 72.08 C 145.58 69.29 148.66 65.92 153.59 60.01 M 142.84 78.48 C 147.35 72.89 151.73 69.65 158.58 60.37 M 142.84 78.48 C 147.18 73.95 151.21 69.08 158.58 60.37 M 143.23 84.12 C 147.37 80.75 153.33 74.19 164.22 59.97 M 143.23 84.12 C 148.03 78.31 154.37 71.08 164.22 59.97 M 142.97 90.52 C 151.59 80.57 159.62 70.52 169.21 60.33 M 142.97 90.52 C 151.28 79.91 159.28 71.54 169.21 60.33 M 142.71 96.92 C 156.01 85.37 167.21 70.9 174.2 60.69 M 142.71 96.92 C 150.4 87.4 159.53 78.24 174.2 60.69 M 143.1 102.56 C 153.97 91.59 164.89 78.96 173.94 67.09 M 143.1 102.56 C 153.28 92.23 161.14 80.99 173.94 67.09 M 142.84 108.96 C 152.12 96.26 165.41 82.19 174.33 72.73 M 142.84 108.96 C 150.17 100.65 156.12 92.68 174.33 72.73 M 143.23 114.6 C 149.82 105.85 154.32 101.85 174.07 79.13 M 143.23 114.6 C 150.94 105.56 157.29 98.38 174.07 79.13 M 142.97 121 C 151.89 112.18 157.5 102.31 173.81 85.53 M 142.97 121 C 151.05 111.02 158.53 102.83 173.81 85.53 M 142.71 127.4 C 148.41 118.98 156.26 111.64 174.2 91.17 M 142.71 127.4 C 152.87 116.12 163.19 102.99 174.2 91.17 M 143.11 133.04 C 153.13 122.76 160.28 114.95 173.94 97.57 M 143.11 133.04 C 153.22 122.63 161.98 111.22 173.94 97.57 M 142.84 139.44 C 152.86 129.74 164.5 114.7 174.34 103.21 M 142.84 139.44 C 150.09 129.79 158.47 120.75 174.34 103.21 M 143.24 145.08 C 153.57 132.68 166.1 115.78 174.07 109.61 M 143.24 145.08 C 154.5 131.34 166.87 118.04 174.07 109.61 M 142.98 151.48 C 148.94 144.42 158.26 134.74 173.81 116.01 M 142.98 151.48 C 151.65 142.1 159.37 134.04 173.81 116.01 M 142.72 157.88 C 151.28 149.18 163.03 137.36 174.21 121.65 M 142.72 157.88 C 153.98 143.92 166.07 130.03 174.21 121.65 M 143.11 163.52 C 153.03 151.57 164.3 142.1 173.95 128.05 M 143.11 163.52 C 148.79 155.34 156.51 147.25 173.95 128.05 M 142.85 169.92 C 152.85 157.91 164.03 143.05 174.34 133.69 M 142.85 169.92 C 152.05 159.59 160.94 150.79 174.34 133.69 M 143.24 175.56 C 153.01 164.67 162.63 152.95 174.08 140.09 M 143.24 175.56 C 150.9 167.24 157.86 158.43 174.08 140.09 M 142.98 181.96 C 153.56 173.13 159.91 162.42 173.82 146.49 M 142.98 181.96 C 150.24 173.32 157.86 164.64 173.82 146.49 M 142.72 188.36 C 149.41 180.1 158.16 173.26 174.21 152.13 M 142.72 188.36 C 149.69 179.14 158.29 171.14 174.21 152.13 M 143.12 194 C 151.56 185.25 161.44 174.52 173.95 158.53 M 143.12 194 C 154.24 181.51 165.31 167.82 173.95 158.53 M 142.85 200.4 C 154.05 187.18 168 170.95 174.35 164.17 M 142.85 200.4 C 152.37 189.01 161.3 179.79 174.35 164.17 M 143.25 206.04 C 153.61 194.25 162.67 181.36 174.08 170.57 M 143.25 206.04 C 155.21 193.37 165.36 179.12 174.08 170.57 M 142.99 212.44 C 149.95 203.44 155.29 196.2 173.82 176.97 M 142.99 212.44 C 154.54 198.63 167.25 184.73 173.82 176.97 M 142.73 218.84 C 150.81 209.11 164.52 196.72 174.22 182.61 M 142.73 218.84 C 150.94 207.28 161.25 196.4 174.22 182.61 M 143.12 224.48 C 152.81 212.81 162.65 200.73 173.96 189.01 M 143.12 224.48 C 149.72 217.01 158.22 208.78 173.96 189.01 M 142.86 230.88 C 148.48 220.47 158.91 214.36 174.35 194.65 M 142.86 230.88 C 155.25 217.61 166.87 204.04 174.35 194.65 M 143.25 236.52 C 152.63 226.92 161.11 214.86 174.09 201.05 M 143.25 236.52 C 149.73 228.71 156.89 220.95 174.09 201.05 M 142.99 242.92 C 150.06 236.93 154.97 226.3 173.83 207.45 M 142.99 242.92 C 154.97 230.59 166.75 216.81 173.83 207.45 M 142.73 249.32 C 153.9 237.63 164.05 222.12 174.22 213.09 M 142.73 249.32 C 151.52 238.79 160.56 227.31 174.22 213.09 M 143.13 254.96 C 156.36 240.48 166.62 226.49 173.96 219.49 M 143.13 254.96 C 150.17 245.29 158.54 237.42 173.96 219.49 M 142.86 261.36 C 155.17 248.38 163.43 235.27 173.7 225.89 M 142.86 261.36 C 151.2 251.53 159.2 242.33 173.7 225.89 M 143.26 267 C 150.84 257.14 159.78 250.78 174.09 231.53 M 143.26 267 C 152.47 255.75 162.75 244.82 174.09 231.53 M 143 273.4 C 155.29 260.86 163.98 247.45 173.83 237.93 M 143 273.4 C 153.72 261.37 164.51 247.89 173.83 237.93 M 142.74 279.8 C 156.22 266.11 168.38 249.83 174.23 243.57 M 142.74 279.8 C 150.3 270.33 157.58 262.11 174.23 243.57 M 143.13 285.44 C 155.63 271.67 165.78 258.19 173.97 249.97 M 143.13 285.44 C 154.25 273.74 165.8 261.2 173.97 249.97 M 142.87 291.84 C 152 281.14 157.33 272.71 173.7 256.37 M 142.87 291.84 C 149.92 283.56 158.82 274.52 173.7 256.37 M 143.26 297.48 C 155.04 286.18 161.36 273.42 174.1 262.01 M 143.26 297.48 C 152.24 286.36 160.38 277.18 174.1 262.01 M 143 303.88 C 154.18 289.99 165.95 274.8 173.84 268.41 M 143 303.88 C 153.99 291.75 164.28 278.1 173.84 268.41 M 142.74 310.28 C 150.67 300.59 157.65 293.03 174.23 274.05 M 142.74 310.28 C 150.88 300.84 157.95 291.68 174.23 274.05 M 143.14 315.92 C 151.68 307.21 162.87 295.07 173.97 280.45 M 143.14 315.92 C 153.71 305.14 163.43 293.18 173.97 280.45 M 142.87 322.32 C 149.19 315.46 155.38 304.82 173.71 286.85 M 142.87 322.32 C 149.75 314.03 157.36 305.35 173.71 286.85 M 143.27 327.96 C 154.26 318.48 161.81 307.55 174.1 292.49 M 143.27 327.96 C 153.79 317.09 162.67 305.85 174.1 292.49 M 143.01 334.36 C 155.68 322.55 168.39 309.11 173.84 298.89 M 143.01 334.36 C 154.35 320.68 166.8 306.82 173.84 298.89 M 142.75 340.76 C 150.28 334.82 155.12 325.82 174.24 304.53 M 142.75 340.76 C 153.67 327.07 165.18 314.59 174.24 304.53 M 143.14 346.4 C 148.66 336.16 157.74 329.6 173.97 310.93 M 143.14 346.4 C 154.95 333.87 165.66 320.75 173.97 310.93 M 142.88 352.8 C 152.56 343.26 158.81 334.52 173.71 317.32 M 142.88 352.8 C 154.04 339.21 165.26 325.7 173.71 317.32 M 143.27 358.44 C 151.22 346.14 161.85 336.26 174.11 322.97 M 143.27 358.44 C 150.53 349.64 158.99 341.65 174.11 322.97 M 143.01 364.84 C 154.54 354.09 164.34 339.84 173.85 329.37 M 143.01 364.84 C 154.53 352.8 165.53 340.49 173.85 329.37 M 142.75 371.23 C 153.96 359.95 163.01 351.67 174.24 335.01 M 142.75 371.23 C 155.03 356.98 166.63 344.34 174.24 335.01 M 143.15 376.88 C 151.04 368.69 161.21 355.42 173.98 341.41 M 143.15 376.88 C 152.02 366.35 161.91 355.57 173.98 341.41 M 142.88 383.28 C 155.72 370.83 165.15 357.29 173.72 347.8 M 142.88 383.28 C 152.24 373.43 161.1 362.94 173.72 347.8 M 143.28 388.92 C 154.22 376.03 163.16 365.21 174.11 353.45 M 143.28 388.92 C 153.22 376.47 164.36 364.82 174.11 353.45 M 143.02 395.32 C 153.68 386.34 160.25 372.56 173.85 359.85 M 143.02 395.32 C 152.45 384.15 162.91 373.89 173.85 359.85 M 143.41 400.96 C 153.28 390.78 159.93 381.63 174.25 365.49 M 143.41 400.96 C 149.96 392.86 156.57 386.42 174.25 365.49 M 149.05 400.56 C 156.49 392.05 159.75 385.59 173.98 371.89 M 149.05 400.56 C 154.1 393.62 160.42 388.12 173.98 371.89 M 154.04 400.93 C 156.99 397.36 161.03 390.57 173.72 378.28 M 154.04 400.93 C 161.85 391.82 168.35 384.1 173.72 378.28 M 159.68 400.53 C 162.96 395.24 170.01 389.14 174.12 383.93 M 159.68 400.53 C 165.25 394.46 170.79 387.01 174.12 383.93 M 164.67 400.89 C 166.24 399.37 168.88 395.24 173.86 390.32 M 164.67 400.89 C 167.24 397.61 170.42 394.31 173.86 390.32 M 170.31 400.5 C 170.91 399.91 171.96 398.77 174.25 395.97 M 170.31 400.5 C 172.08 398.89 173.64 397.26 174.25 395.97"
+      fill="none"
+      stroke="#999999"
+      stroke-linejoin="round"
+      stroke-linecap="round"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+    <path
+      d="M 143 60 C 148.22 61.33 156.47 58.92 173 60 M 143 60 C 150.13 60.9 158.74 59.47 173 60 M 173 60 C 171.86 172.31 170.18 286.27 173 400 M 173 60 C 171.27 157.93 171.37 256.76 173 400 M 173 400 C 162.07 398.64 151.91 401.46 143 400 M 173 400 C 161.43 399.95 150.53 399.36 143 400 M 143 400 C 144.46 289.28 143.12 178.24 143 60 M 143 400 C 144.73 328.35 145.03 257.25 143 60"
+      fill="none"
+      stroke="#000000"
+      stroke-linejoin="round"
+      stroke-linecap="round"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+    <path d="M 0 0 L 173 0 L 173 40 L 134.61 40 L 150.51 70 L 98.61 40 L 0 40 Z" fill="#999999" stroke="#4d4d4d" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 171px; height: 1px; padding-top: 20px; margin-left: 1px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">
+                <div style="text-align: left">
+                  <span style="color: rgb(0 , 0 , 0)">Unaccessible area</span>
+                </div>
+                <div style="text-align: left">
+                  <font color="#000000">(e.g. fences, plants, trees, etc.)</font>
+                  <br/>
+                </div>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="87" y="24" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Unaccessible area...</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Viewer does not support full SVG 1.1</text>
+    </a>
+  </switch>
+</svg>
diff --git a/lanelet2_extension/docs/right_of_way.drawio.svg b/lanelet2_extension/docs/right_of_way.drawio.svg
new file mode 100644
index 0000000..6a1a172
--- /dev/null
+++ b/lanelet2_extension/docs/right_of_way.drawio.svg
@@ -0,0 +1,384 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- Do not edit this file with editors other than diagrams.net -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="1555px"
+  height="1291px"
+  viewBox="-0.5 -0.5 1555 1291"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2022-10-27T03:17:08.087Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36&quot; etag=&quot;M8ZSj-BtGpXJRytN2iO1&quot; version=&quot;20.3.0&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;C5H5OodgWkqNMdUJiKq4&quot; name=&quot;Page-1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <path d="M 0.47 176 L 160.51 176 L 160.47 16" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 0.47 416 L 160.51 416 L 160.47 576" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 560.47 416 L 400.51 416 L 400.47 576" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 560.47 176 L 400.51 176 L 400.47 16" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="stroke"/>
+    <rect x="-90" y="276.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,190.15,296.15)" pointer-events="all"/>
+    <rect x="-29.53" y="276.31" width="560" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,250.47,296)" pointer-events="all"/>
+    <rect x="30" y="276.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,310.15,296.15)" pointer-events="all"/>
+    <rect x="90.47" y="276.31" width="560" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,370.47,296)" pointer-events="all"/>
+    <rect x="0" y="186" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="all"/>
+    <rect x="0.47" y="246" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="all"/>
+    <rect x="0.47" y="306" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="all"/>
+    <rect x="0.47" y="366" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="all"/>
+    <path d="M 10.07 376 L 151.67 376 L 199.67 376 L 199.67 406 L 199.67 566 L 180.47 566 L 180.47 396 L 10.07 396 Z" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <path
+      d="M 350.07 356 L 491.67 356 L 539.67 356 L 539.67 386 L 539.67 546 L 520.47 546 L 520.47 376 L 350.07 376 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-90,460.47,456)"
+      pointer-events="all"
+    />
+    <path
+      d="M -9.93 36 L 131.67 36 L 179.67 36 L 179.67 66 L 179.67 226 L 160.47 226 L 160.47 56 L -9.93 56 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(90,100.47,136)"
+      pointer-events="all"
+    />
+    <path
+      d="M 330.38 16 L 471.98 16 L 519.98 16 L 519.98 46 L 519.98 206 L 500.78 206 L 500.78 36 L 330.38 36 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-180,440.78,116)"
+      pointer-events="all"
+    />
+    <path
+      d="M 235.47 417 C 235.47 292.18 338.22 191 464.97 191 L 464.97 220.38 C 354.7 220.38 265.31 308.41 265.31 417 Z"
+      fill-opacity="0.8"
+      fill="#dae8fc"
+      stroke="#6c8ebf"
+      stroke-opacity="0.8"
+      stroke-miterlimit="10"
+      pointer-events="all"
+    />
+    <path
+      d="M 10.47 172 C 10.47 80.32 79.64 6 164.97 6 L 164.97 32.56 C 93.29 32.56 35.19 94.99 35.19 172 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-180,164.97,172)"
+      pointer-events="all"
+    />
+    <path d="M 280.45 426.04 L 280.47 166" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="stroke"/>
+    <path d="M 150.47 296 L 400.25 295.86" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="stroke"/>
+    <rect x="145.47" y="141" width="50" height="20" rx="3" ry="3" fill="none" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <ellipse cx="154.31" cy="150.05" rx="5.7142857142857135" ry="5.7142857142857135" fill="#60a917" stroke="#2d7600" pointer-events="none"/>
+    <ellipse cx="169.93" cy="150.05" rx="5.7142857142857135" ry="5.7142857142857135" fill="#e3c800" stroke="#b09500" pointer-events="none"/>
+    <ellipse cx="185.56" cy="150.05" rx="5.7142857142857135" ry="5.7142857142857135" fill="#e51400" stroke="#b20000" pointer-events="none"/>
+    <path d="M 800.94 176 L 960.88 176 L 960.94 16" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 800.94 416 L 960.88 416 L 960.94 576" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 1360.94 416 L 1200.88 416 L 1200.94 576" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 1360.94 176 L 1200.88 176 L 1200.94 16" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <rect x="710.47" y="276.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,990.63,296.15)" pointer-events="none"/>
+    <rect x="770.94" y="276.31" width="560" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,1050.94,296)" pointer-events="none"/>
+    <rect x="830.47" y="276.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,1110.63,296.15)" pointer-events="none"/>
+    <rect x="890.94" y="276.31" width="560" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,1170.94,296)" pointer-events="none"/>
+    <rect x="800" y="186" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="800.94" y="246" width="560.31" height="39.38" fill="none" stroke="#000000" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="800.94" y="306" width="560.31" height="39.38" fill="none" stroke="#000000" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="800.94" y="366" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="none"/>
+    <path
+      d="M 810.54 376 L 952.14 376 L 1000.14 376 L 1000.14 406 L 1000.14 566 L 980.94 566 L 980.94 396 L 810.54 396 Z"
+      fill="#dae8fc"
+      stroke="#6c8ebf"
+      stroke-miterlimit="10"
+      pointer-events="none"
+    />
+    <path
+      d="M 1150.54 356 L 1292.14 356 L 1340.14 356 L 1340.14 386 L 1340.14 546 L 1320.94 546 L 1320.94 376 L 1150.54 376 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-90,1260.94,456)"
+      pointer-events="none"
+    />
+    <path
+      d="M 790.54 36 L 932.14 36 L 980.14 36 L 980.14 66 L 980.14 226 L 960.94 226 L 960.94 56 L 790.54 56 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(90,900.94,136)"
+      pointer-events="none"
+    />
+    <path
+      d="M 1130.85 16 L 1272.45 16 L 1320.45 16 L 1320.45 46 L 1320.45 206 L 1301.25 206 L 1301.25 36 L 1130.85 36 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-180,1241.25,116)"
+      pointer-events="none"
+    />
+    <path d="M 1080.92 426.04 L 1080.94 166" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="none"/>
+    <path d="M 950.94 296 L 1200.72 295.86" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="none"/>
+    <rect x="945.94" y="141" width="50" height="20" rx="3" ry="3" fill="none" stroke="rgb(0, 0, 0)" pointer-events="none"/>
+    <ellipse cx="954.78" cy="150.05" rx="5.7142857142857135" ry="5.7142857142857135" fill="#60a917" stroke="#2d7600" pointer-events="none"/>
+    <ellipse cx="970.4" cy="150.05" rx="5.7142857142857135" ry="5.7142857142857135" fill="#e3c800" stroke="#b09500" pointer-events="none"/>
+    <ellipse cx="986.03" cy="150.05" rx="5.7142857142857135" ry="5.7142857142857135" fill="#e51400" stroke="#b20000" pointer-events="none"/>
+    <path
+      d="M 275.47 388.43 L 275.47 365.6 L 273.41 367.4 L 273.41 390.6 Z M 273.41 390.6 L 273.41 367.4 L 230.47 367.4 L 230.47 390.6 Z M 275.47 365.6 L 232.87 365.6 L 230.47 367.4 L 273.41 367.4 Z M 275.47 365.6 L 273.41 367.4"
+      fill="#036897"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-linejoin="round"
+      stroke-miterlimit="10"
+      transform="rotate(-80,252.97,378.1)"
+      pointer-events="none"
+    />
+    <rect x="230.47" y="365.6" width="0" height="0" fill="none" stroke="#ffffff" stroke-width="2" transform="rotate(-80,252.97,378.1)" pointer-events="none"/>
+    <path
+      d="M 255.54 371.75 C 257.95 369.58 257.95 369.58 257.95 369.58 C 265.85 379.36 259.66 386.97 259.66 386.97 C 270.32 386.97 270.32 386.97 270.32 386.97 M 238.71 373.93 C 238.71 371.75 240.77 371.39 240.77 371.39 C 240.77 371.39 252.11 371.39 254.51 371.39 C 257.26 371.39 256.92 374.3 256.92 374.3 C 256.92 374.3 256.92 384.08 256.92 385.52 C 256.92 387.34 254.86 388.06 254.86 388.06 C 254.86 388.06 243.18 388.06 241.11 388.06 C 239.4 388.06 238.71 385.88 238.71 385.88 Z M 270.32 372.48 L 259.66 372.48 M 236.99 372.48 L 231.84 372.48"
+      fill="none"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-miterlimit="10"
+      transform="rotate(-80,252.97,378.1)"
+      pointer-events="none"
+    />
+    <path
+      d="M 1010 423.83 L 1010 401 L 1007.94 402.8 L 1007.94 426 Z M 1007.94 426 L 1007.94 402.8 L 965 402.8 L 965 426 Z M 1010 401 L 967.4 401 L 965 402.8 L 1007.94 402.8 Z M 1010 401 L 1007.94 402.8"
+      fill="#036897"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-linejoin="round"
+      stroke-miterlimit="10"
+      transform="rotate(-105,987.5,413.5)"
+      pointer-events="none"
+    />
+    <rect x="965" y="401" width="0" height="0" fill="none" stroke="#ffffff" stroke-width="2" transform="rotate(-105,987.5,413.5)" pointer-events="none"/>
+    <path
+      d="M 990.07 407.15 C 992.48 404.98 992.48 404.98 992.48 404.98 C 1000.38 414.76 994.19 422.37 994.19 422.37 C 1004.85 422.37 1004.85 422.37 1004.85 422.37 M 973.24 409.33 C 973.24 407.15 975.3 406.79 975.3 406.79 C 975.3 406.79 986.64 406.79 989.04 406.79 C 991.79 406.79 991.45 409.7 991.45 409.7 C 991.45 409.7 991.45 419.48 991.45 420.92 C 991.45 422.74 989.39 423.46 989.39 423.46 C 989.39 423.46 977.71 423.46 975.64 423.46 C 973.93 423.46 973.24 421.28 973.24 421.28 Z M 1004.85 407.88 L 994.19 407.88 M 971.52 407.88 L 966.37 407.88"
+      fill="none"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-miterlimit="10"
+      transform="rotate(-105,987.5,413.5)"
+      pointer-events="none"
+    />
+    <path d="M 0.47 889 L 160.51 889.02 L 160.47 729" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 0.47 1129 L 160.51 1129.02 L 160.47 1289" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 560.47 1129 L 400.51 1129.02 L 400.47 1289" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 560.47 889 L 400.51 889.02 L 400.47 729" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <rect x="-90" y="989.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,190.15,1009.15)" pointer-events="none"/>
+    <rect x="-29.53" y="989.31" width="560" height="39.38" fill="#dae8fc" stroke="#6c8ebf" transform="rotate(90,250.47,1009)" pointer-events="none"/>
+    <rect x="30" y="989.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,310.15,1009.15)" pointer-events="none"/>
+    <rect x="90.47" y="989.31" width="560" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,370.47,1009)" pointer-events="none"/>
+    <rect x="0" y="899" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="0.47" y="959" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="0.47" y="1019" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="0.47" y="1079" width="560.31" height="39.38" fill-opacity="0.8" fill="#fff2cc" stroke="#d6b656" stroke-opacity="0.8" pointer-events="none"/>
+    <path
+      d="M 10.07 1089 L 151.67 1089 L 199.67 1089 L 199.67 1119 L 199.67 1279 L 180.47 1279 L 180.47 1109 L 10.07 1109 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      pointer-events="none"
+    />
+    <path
+      d="M 350.07 1069 L 491.67 1069 L 539.67 1069 L 539.67 1099 L 539.67 1259 L 520.47 1259 L 520.47 1089 L 350.07 1089 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-90,460.47,1169)"
+      pointer-events="none"
+    />
+    <path
+      d="M -9.93 749 L 131.67 749 L 179.67 749 L 179.67 779 L 179.67 939 L 160.47 939 L 160.47 769 L -9.93 769 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(90,100.47,849)"
+      pointer-events="none"
+    />
+    <path
+      d="M 330.38 729 L 471.98 729 L 519.98 729 L 519.98 759 L 519.98 919 L 500.78 919 L 500.78 749 L 330.38 749 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-180,440.78,829)"
+      pointer-events="none"
+    />
+    <path
+      d="M 10.47 885 C 10.47 793.32 79.64 719 164.97 719 L 164.97 745.56 C 93.29 745.56 35.19 807.99 35.19 885 Z"
+      fill="#fff2cc"
+      stroke="#d6b656"
+      stroke-miterlimit="10"
+      transform="rotate(-180,164.97,885)"
+      pointer-events="none"
+    />
+    <path d="M 280.45 1139.04 L 280.47 879" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="none"/>
+    <path d="M 150.47 1009 L 400.25 1008.86" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="none"/>
+    <path
+      d="M 275 1168.83 L 275 1146 L 272.94 1147.8 L 272.94 1171 Z M 272.94 1171 L 272.94 1147.8 L 230 1147.8 L 230 1171 Z M 275 1146 L 232.4 1146 L 230 1147.8 L 272.94 1147.8 Z M 275 1146 L 272.94 1147.8"
+      fill="#036897"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-linejoin="round"
+      stroke-miterlimit="10"
+      transform="rotate(-90,252.5,1158.5)"
+      pointer-events="none"
+    />
+    <rect x="230" y="1146" width="0" height="0" fill="none" stroke="#ffffff" stroke-width="2" transform="rotate(-90,252.5,1158.5)" pointer-events="none"/>
+    <path
+      d="M 255.07 1152.15 C 257.48 1149.98 257.48 1149.98 257.48 1149.98 C 265.38 1159.76 259.19 1167.37 259.19 1167.37 C 269.85 1167.37 269.85 1167.37 269.85 1167.37 M 238.24 1154.33 C 238.24 1152.15 240.3 1151.79 240.3 1151.79 C 240.3 1151.79 251.64 1151.79 254.04 1151.79 C 256.79 1151.79 256.45 1154.7 256.45 1154.7 C 256.45 1154.7 256.45 1164.48 256.45 1165.92 C 256.45 1167.74 254.39 1168.46 254.39 1168.46 C 254.39 1168.46 242.71 1168.46 240.64 1168.46 C 238.93 1168.46 238.24 1166.28 238.24 1166.28 Z M 269.85 1152.88 L 259.19 1152.88 M 236.52 1152.88 L 231.37 1152.88"
+      fill="none"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-miterlimit="10"
+      transform="rotate(-90,252.5,1158.5)"
+      pointer-events="none"
+    />
+    <rect x="142.68" y="803.68" width="37.07" height="37.07" rx="5.56" ry="5.56" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(-45,161.22,822.21)" pointer-events="none"/>
+    <rect x="151.95" y="813.59" width="18.54" height="18.54" fill="#e3c800" stroke="#b09500" transform="rotate(-45,161.22,822.86)" pointer-events="none"/>
+    <path d="M 800.47 889 L 960.51 889.02 L 960.47 729" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 800.47 1129 L 960.51 1129.02 L 960.47 1289" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 1360.47 1129 L 1200.51 1129.02 L 1200.47 1289" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <path d="M 1360.47 889 L 1200.51 889.02 L 1200.47 729" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" pointer-events="none"/>
+    <rect x="710" y="989.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,990.15,1009.15)" pointer-events="none"/>
+    <rect x="770.47" y="989.31" width="560" height="39.38" fill="#dae8fc" stroke="#6c8ebf" transform="rotate(90,1050.47,1009)" pointer-events="none"/>
+    <rect x="830" y="989.46" width="560.31" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,1110.15,1009.15)" pointer-events="none"/>
+    <rect x="890.47" y="989.31" width="560" height="39.38" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(90,1170.47,1009)" pointer-events="none"/>
+    <rect x="800" y="899" width="560.31" height="39.38" fill="none" stroke="#000000" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="800.47" y="959" width="560.31" height="39.38" fill="none" stroke="#000000" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="800.47" y="1019" width="560.31" height="39.38" fill="none" stroke="#000000" stroke-opacity="0.8" pointer-events="none"/>
+    <rect x="800.47" y="1079" width="560.31" height="39.38" fill="none" stroke="#000000" stroke-opacity="0.8" pointer-events="none"/>
+    <path
+      d="M 810.07 1089 L 951.67 1089 L 999.67 1089 L 999.67 1119 L 999.67 1279 L 980.47 1279 L 980.47 1109 L 810.07 1109 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      pointer-events="none"
+    />
+    <path
+      d="M 1150.07 1069 L 1291.67 1069 L 1339.67 1069 L 1339.67 1099 L 1339.67 1259 L 1320.47 1259 L 1320.47 1089 L 1150.07 1089 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-90,1260.47,1169)"
+      pointer-events="none"
+    />
+    <path
+      d="M 790.07 749 L 931.67 749 L 979.67 749 L 979.67 779 L 979.67 939 L 960.47 939 L 960.47 769 L 790.07 769 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(90,900.47,849)"
+      pointer-events="none"
+    />
+    <path
+      d="M 1130.38 729 L 1271.98 729 L 1319.98 729 L 1319.98 759 L 1319.98 919 L 1300.78 919 L 1300.78 749 L 1130.38 749 Z"
+      fill="none"
+      stroke="rgb(0, 0, 0)"
+      stroke-miterlimit="10"
+      transform="rotate(-180,1240.78,829)"
+      pointer-events="none"
+    />
+    <path
+      d="M 810.47 885 C 810.47 793.32 879.64 719 964.97 719 L 964.97 745.56 C 893.29 745.56 835.19 807.99 835.19 885 Z"
+      fill="none"
+      stroke="#000000"
+      stroke-miterlimit="10"
+      transform="rotate(-180,964.97,885)"
+      pointer-events="none"
+    />
+    <path d="M 1080.45 1139.04 L 1080.47 879" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="none"/>
+    <path d="M 950.47 1009 L 1200.25 1008.86" fill="none" stroke="rgb(0, 0, 0)" stroke-width="2" stroke-miterlimit="10" stroke-dasharray="2 6" pointer-events="none"/>
+    <path
+      d="M 1075 1168.83 L 1075 1146 L 1072.94 1147.8 L 1072.94 1171 Z M 1072.94 1171 L 1072.94 1147.8 L 1030 1147.8 L 1030 1171 Z M 1075 1146 L 1032.4 1146 L 1030 1147.8 L 1072.94 1147.8 Z M 1075 1146 L 1072.94 1147.8"
+      fill="#036897"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-linejoin="round"
+      stroke-miterlimit="10"
+      transform="rotate(-90,1052.5,1158.5)"
+      pointer-events="none"
+    />
+    <rect x="1030" y="1146" width="0" height="0" fill="none" stroke="#ffffff" stroke-width="2" transform="rotate(-90,1052.5,1158.5)" pointer-events="none"/>
+    <path
+      d="M 1055.07 1152.15 C 1057.48 1149.98 1057.48 1149.98 1057.48 1149.98 C 1065.38 1159.76 1059.19 1167.37 1059.19 1167.37 C 1069.85 1167.37 1069.85 1167.37 1069.85 1167.37 M 1038.24 1154.33 C 1038.24 1152.15 1040.3 1151.79 1040.3 1151.79 C 1040.3 1151.79 1051.64 1151.79 1054.04 1151.79 C 1056.79 1151.79 1056.45 1154.7 1056.45 1154.7 C 1056.45 1154.7 1056.45 1164.48 1056.45 1165.92 C 1056.45 1167.74 1054.39 1168.46 1054.39 1168.46 C 1054.39 1168.46 1042.71 1168.46 1040.64 1168.46 C 1038.93 1168.46 1038.24 1166.28 1038.24 1166.28 Z M 1069.85 1152.88 L 1059.19 1152.88 M 1036.52 1152.88 L 1031.37 1152.88"
+      fill="none"
+      stroke="#ffffff"
+      stroke-width="2"
+      stroke-miterlimit="10"
+      transform="rotate(-90,1052.5,1158.5)"
+      pointer-events="none"
+    />
+    <rect x="892.68" y="868.68" width="37.07" height="37.07" rx="5.56" ry="5.56" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(-45,911.21,887.21)" pointer-events="none"/>
+    <rect x="901.95" y="878.59" width="18.54" height="18.54" fill="#e3c800" stroke="#b09500" transform="rotate(-45,911.22,887.86)" pointer-events="none"/>
+    <rect x="1395" y="7" width="42" height="28" rx="4.2" ry="4.2" fill="#fff2cc" stroke="#000000" pointer-events="none"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 21px; margin-left: 1493px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: none; white-space: nowrap;">
+                <font style="font-size: 18px;">yield lane</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1493" y="25" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">yield lane</text>
+      </switch>
+    </g>
+    <rect x="1395" y="49" width="42" height="28" rx="4.2" ry="4.2" fill="#dae8fc" stroke="#000000" pointer-events="none"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 21px; margin-left: 1493px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: none; white-space: nowrap;">
+                <font style="font-size: 18px;">yield lane</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1493" y="25" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">yield lane</text>
+      </switch>
+    </g>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 63px; margin-left: 1494px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: none; white-space: nowrap;">
+                <font style="font-size: 18px;">ego lane</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1494" y="67" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">ego lane</text>
+      </switch>
+    </g>
+    <rect x="1401.15" y="96.3" width="29.7" height="30.41" rx="4.45" ry="4.45" fill="none" stroke="rgb(0, 0, 0)" transform="rotate(-45,1416,111.5)" pointer-events="none"/>
+    <rect x="1408.58" y="104.43" width="14.85" height="15.2" fill="#e3c800" stroke="#b09500" transform="rotate(-45,1416,112.03)" pointer-events="none"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 112px; margin-left: 1497px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: none; white-space: nowrap;">
+                <font style="font-size: 18px;">priority lane</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="1497" y="116" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">priority lane</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>
diff --git a/lanelet2_extension/docs/road_mark.png b/lanelet2_extension/docs/road_mark.png
new file mode 100644
index 0000000..98b4102
Binary files /dev/null and b/lanelet2_extension/docs/road_mark.png differ
diff --git a/lanelet2_extension/docs/road_shoulder.svg b/lanelet2_extension/docs/road_shoulder.svg
new file mode 100644
index 0000000..489b83c
--- /dev/null
+++ b/lanelet2_extension/docs/road_shoulder.svg
@@ -0,0 +1,81 @@
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="471px"
+  height="393px"
+  viewBox="-0.5 -0.5 471 393"
+  content="&lt;mxfile&gt;&lt;diagram id=&quot;k8MIM5X1AGlBXXJi1RH8&quot; name=&quot;Page-1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <rect x="130" y="50" width="340" height="340" fill="#666666" stroke="#4d4d4d" pointer-events="all"/>
+    <path d="M 215 390 L 215 50" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 385 390 L 385 50" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 300 390 L 300 50" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" stroke-dasharray="9 9" pointer-events="stroke"/>
+    <rect x="162" y="50" width="51" height="340" fill-opacity="0.3" fill="#99ff33" stroke="none" pointer-events="all"/>
+    <path d="M 162 390 L 162 50" fill="none" stroke="#F0F0F0" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 196 0 L 336 0 L 336 40 L 256.6 40 L 205.8 70 L 236.6 40 L 196 40 Z" fill="#99ff33" stroke="#4d4d4d" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 138px; height: 1px; padding-top: 20px; margin-left: 197px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">
+                <div style="text-align: left">
+                  <span style="color: rgb(0 , 0 , 0)">type: lanelet</span>
+                </div>
+                <font color="#000000">
+                  <div style="text-align: left">
+                    <span>subtype: road_shoulder</span>
+                  </div>
+                </font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="266" y="24" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">type: lanelet subtype: r...</text>
+      </switch>
+    </g>
+    <rect x="116" y="50" width="44" height="340" fill="#666666" stroke="#000000" pointer-events="all"/>
+    <path d="M 0 0 L 179 0 L 179 40 L 116.92 40 L 136.04 70 L 85.92 40 L 0 40 Z" fill="#999999" stroke="#4d4d4d" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 177px; height: 1px; padding-top: 20px; margin-left: 1px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">
+                <div style="text-align: left">
+                  <font color="#000000">Accessible area</font>
+                </div>
+                <div style="text-align: left">
+                  <font color="#000000">(e.g. sidewalks, bike lanes, etc.)</font>
+                  <br/>
+                </div>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="90" y="24" fill="#000000" font-family="Helvetica" font-size="12px" text-anchor="middle">Accessible area...</text>
+      </switch>
+    </g>
+    <image
+      x="115.07"
+      y="139.93"
+      width="44.5"
+      height="32.63"
+      xlink:href="data:image/png;base64,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"
+      preserveAspectRatio="none"
+      transform="rotate(90,137.82,156.75)"
+    />
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Viewer does not support full SVG 1.1</text>
+    </a>
+  </switch>
+</svg>
diff --git a/lanelet2_extension/docs/speed_bump.png b/lanelet2_extension/docs/speed_bump.png
new file mode 100644
index 0000000..3684e9f
Binary files /dev/null and b/lanelet2_extension/docs/speed_bump.png differ
diff --git a/lanelet2_extension/docs/traffic_light.png b/lanelet2_extension/docs/traffic_light.png
new file mode 100644
index 0000000..aa97c7b
Binary files /dev/null and b/lanelet2_extension/docs/traffic_light.png differ
diff --git a/lanelet2_extension/docs/traffic_light_regulatory_element.png b/lanelet2_extension/docs/traffic_light_regulatory_element.png
new file mode 100644
index 0000000..17b7567
Binary files /dev/null and b/lanelet2_extension/docs/traffic_light_regulatory_element.png differ
diff --git a/lanelet2_extension/docs/turn_direction.png b/lanelet2_extension/docs/turn_direction.png
new file mode 100644
index 0000000..3fd6358
Binary files /dev/null and b/lanelet2_extension/docs/turn_direction.png differ
diff --git a/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp b/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp
new file mode 100644
index 0000000..8645ddb
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp
@@ -0,0 +1,63 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_
+#define LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_io/io_handlers/OsmHandler.h>
+
+#include <memory>
+#include <string>
+
+namespace lanelet::io_handlers
+{
+class AutowareOsmParser : public OsmParser
+{
+public:
+  using OsmParser::OsmParser;
+
+  /**
+   * [parse parse osm file to laneletMap. It is generally same as default
+   * OsmParser, but it will overwrite x and y value with local_x and local_y
+   * tags if present]
+   * @param  filename [path to osm file]
+   * @param  errors   [any errors caught during parsing]
+   * @return          [returns LaneletMap]
+   */
+  std::unique_ptr<LaneletMap> parse(
+    const std::string & filename, ErrorMessages & errors) const override;
+
+  /**
+   * [parseVersions parses MetaInfo tags from osm file]
+   * @param filename       [path to osm file]
+   * @param format_version [parsed information about map format version]
+   * @param map_version    [parsed information about map version]
+   */
+  static void parseVersions(
+    const std::string & filename, std::string * format_version, std::string * map_version);
+
+  static constexpr const char * extension() { return ".osm"; }
+
+  static constexpr const char * name() { return "autoware_osm_handler"; }
+};
+
+}  // namespace lanelet::io_handlers
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__IO__AUTOWARE_OSM_PARSER_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/localization/landmark.hpp b/lanelet2_extension/include/lanelet2_extension/localization/landmark.hpp
new file mode 100644
index 0000000..7c96cef
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/localization/landmark.hpp
@@ -0,0 +1,38 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__LOCALIZATION__LANDMARK_HPP_
+#define LANELET2_EXTENSION__LOCALIZATION__LANDMARK_HPP_
+
+#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
+
+#include <lanelet2_core/primitives/Polygon.h>
+
+#include <string>
+#include <vector>
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+namespace lanelet::localization
+{
+
+std::vector<lanelet::Polygon3d> parseLandmarkPolygons(
+  const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
+  const std::string & target_subtype);
+
+}  // namespace lanelet::localization
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__LOCALIZATION__LANDMARK_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp b/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp
new file mode 100644
index 0000000..37b6bc0
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp
@@ -0,0 +1,116 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_
+#define LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
+#include <lanelet2_io/Exceptions.h>
+#include <lanelet2_io/Projection.h>
+
+#include <string>
+#include <utility>
+
+namespace lanelet::projection
+{
+class MGRSProjector : public Projector
+{
+public:
+  explicit MGRSProjector(Origin origin = Origin({0.0, 0.0}));
+
+  /**
+   * [MGRSProjector::forward projects gps lat/lon to MGRS 100km grid]
+   * @param  gps [point with latitude longitude information]
+   * @return     [projected point in MGRS coordinate]
+   */
+  BasicPoint3d forward(const GPSPoint & gps) const override;
+
+  /**
+   * [MGRSProjector::forward projects gps lat/lon to MGRS xyz coordinate]
+   * @param  gps       [point with latitude longitude information]
+   * @param  precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m,
+   * 4=10m, 5=1m]
+   * @return           [projected point in MGRS coordinate]
+   */
+  BasicPoint3d forward(const GPSPoint & gps, const int precision) const;
+
+  /**
+   * [MGRSProjector::reverse projects point within MGRS 100km grid into gps
+   * lat/lon (WGS84)]
+   * @param  mgrs_point [3d point in MGRS 100km grid]
+   * @return            [projected point in WGS84]
+   */
+  GPSPoint reverse(const BasicPoint3d & mgrs) const override;
+
+  /**
+   * [MGRSProjector::reverse projects point within MGRS grid into gps lat/lon
+   * (WGS84)]
+   * @param  mgrs_point [3d point in MGRS grid]
+   * @param  mgrs_code  [MGRS grid code]
+   * @return            [projected point in WGS84]
+   */
+  static GPSPoint reverse(const BasicPoint3d & mgrs_point, const std::string & mgrs_code);
+
+  /**
+   * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection]
+   * @param mgrs_code [MGRS code. Minimum requirement is GZD and 100 km Grid
+   * Square ID. e.g. "4QFJ"]
+   */
+  void setMGRSCode(const std::string & mgrs_code);
+
+  /**
+   * [MGRSProjector::setMGRSCode sets MGRS code used for reverse projection from
+   * gps lat/lon values]
+   * @param gps       [gps point used to find GMRS Grid]
+   * @param precision [resolution of MGRS Grid 0=100km, 1=10km, 2=1km, 3=100m,
+   * 4=10m, 5=1m]
+   */
+  void setMGRSCode(const GPSPoint & gps, const int precision = 0);
+
+  /**
+   * [getProjectedMGRSGrid returns mgrs]
+   * @return [description]
+   */
+  std::string getProjectedMGRSGrid() const { return projected_grid_; }
+
+  /**
+   * [isMGRSCodeSet checks if mgrs code is set for reverse projection]
+   * @return [true if mgrs_code member is set]
+   */
+  bool isMGRSCodeSet() const { return !mgrs_code_.empty(); }
+
+private:
+  /**
+   * mgrs grid code used for reverse function
+   */
+  std::string mgrs_code_;
+
+  /**
+   * mgrs grid code that was last projected in previous forward function.
+   * reverse function will use this if isMGRSCodeSet() returns false.
+   */
+  mutable std::string projected_grid_;
+};
+
+}  // namespace lanelet::projection
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__PROJECTION__MGRS_PROJECTOR_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/projection/transverse_mercator_projector.hpp b/lanelet2_extension/include/lanelet2_extension/projection/transverse_mercator_projector.hpp
new file mode 100644
index 0000000..f8693b9
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/projection/transverse_mercator_projector.hpp
@@ -0,0 +1,61 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__PROJECTION__TRANSVERSE_MERCATOR_PROJECTOR_HPP_
+#define LANELET2_EXTENSION__PROJECTION__TRANSVERSE_MERCATOR_PROJECTOR_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming, modernize-use-nodiscard)
+
+#include <GeographicLib/TransverseMercatorExact.hpp>
+
+#include <lanelet2_io/Exceptions.h>
+#include <lanelet2_io/Projection.h>
+
+#include <string>
+#include <utility>
+
+namespace lanelet::projection
+{
+class TransverseMercatorProjector : public Projector
+{
+public:
+  explicit TransverseMercatorProjector(Origin origin = Origin({0.0, 0.0}));
+
+  /**
+   * [TransverseMercatorProjector::forward projects gps lat/lon to Transverse Mercator coordinate]
+   * @param  gps [point with latitude longitude information]
+   * @return     [projected point in Transverse Mercator coordinate]
+   */
+  BasicPoint3d forward(const GPSPoint & gps) const override;
+
+  /**
+   * @param  local_point [3d point in Transverse Mercator coordinate]
+   * @return            [projected point in WGS84]
+   */
+  GPSPoint reverse(const BasicPoint3d & local_point) const override;
+
+private:
+  /**
+   * origin geoid coordinate for reverse function
+   */
+  double origin_x_;
+  double origin_y_;
+  double central_meridian_;
+};
+
+}  // namespace lanelet::projection
+
+// NOLINTEND(readability-identifier-naming, modernize-use-nodiscard)
+
+#endif  // LANELET2_EXTENSION__PROJECTION__TRANSVERSE_MERCATOR_PROJECTOR_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/Forward.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/Forward.hpp
new file mode 100644
index 0000000..16b814d
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/Forward.hpp
@@ -0,0 +1,54 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__FORWARD_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__FORWARD_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_core/Attribute.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+
+class AutowareTrafficLight;
+class Crosswalk;
+class DetectionArea;
+class NoParkingArea;
+class NoStoppingArea;
+class RoadMarking;
+class SpeedBump;
+class VirtualTrafficLight;
+
+}  // namespace lanelet::autoware
+
+namespace lanelet
+{
+using TrafficSignConstPtr = std::shared_ptr<const lanelet::TrafficSign>;
+using TrafficLightConstPtr = std::shared_ptr<const lanelet::TrafficLight>;
+using AutowareTrafficLightConstPtr = std::shared_ptr<const lanelet::autoware::AutowareTrafficLight>;
+using DetectionAreaConstPtr = std::shared_ptr<const lanelet::autoware::DetectionArea>;
+using NoParkingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoParkingArea>;
+using NoStoppingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoStoppingArea>;
+using NoParkingAreaConstPtr = std::shared_ptr<const lanelet::autoware::NoParkingArea>;
+using SpeedBumpConstPtr = std::shared_ptr<const lanelet::autoware::SpeedBump>;
+using CrosswalkConstPtr = std::shared_ptr<const lanelet::autoware::Crosswalk>;
+}  // namespace lanelet
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__FORWARD_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp
new file mode 100644
index 0000000..f175d23
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp
@@ -0,0 +1,91 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class AutowareTrafficLight : public lanelet::TrafficLight
+{
+public:
+  using Ptr = std::shared_ptr<AutowareTrafficLight>;
+  static constexpr char RuleName[] = "traffic_light";
+
+  struct AutowareRoleNameString
+  {
+    static constexpr const char LightBulbs[] = "light_bulbs";
+  };
+
+  //! Directly construct a stop line from its required rule parameters.
+  //! Might modify the input data in oder to get correct tags.
+  static Ptr make(
+    Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights,
+    const Optional<LineString3d> & stopLine = {}, const LineStrings3d & lightBulbs = {})
+  {
+    return Ptr{new AutowareTrafficLight(id, attributes, trafficLights, stopLine, lightBulbs)};
+  }
+
+  /**
+   * @brief get the relevant traffic light bulbs
+   * @return the traffic light bulbs
+   *
+   * There might be multiple traffic light bulbs but they are required to show
+   * the same signal.
+   */
+  [[nodiscard]] ConstLineStrings3d lightBulbs() const;
+
+  /**
+   * @brief add a new traffic light bulb
+   * @param primitive the traffic light bulb to add
+   *
+   * Traffic light bulbs are represented as linestrings with each point
+   * expressing position of each light bulb (lamp).
+   */
+  void addLightBulbs(const LineStringOrPolygon3d & primitive);
+
+  /**
+   * @brief remove a traffic light bulb
+   * @param primitive the primitive
+   * @return true if the traffic light bulb existed and was removed
+   */
+  bool removeLightBulbs(const LineStringOrPolygon3d & primitive);
+
+private:
+  AutowareTrafficLight(
+    Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights,
+    const Optional<LineString3d> & stopLine, const LineStrings3d & lightBulbs);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<AutowareTrafficLight>;
+  explicit AutowareTrafficLight(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__AUTOWARE_TRAFFIC_LIGHT_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp
new file mode 100644
index 0000000..f6cec39
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp
@@ -0,0 +1,92 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__CROSSWALK_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__CROSSWALK_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class Crosswalk : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<Crosswalk>;
+  static constexpr char RuleName[] = "crosswalk";
+
+  struct AutowareRoleNameString
+  {
+    static constexpr const char CrosswalkPolygon[] = "crosswalk_polygon";
+  };
+
+  static Ptr make(
+    Id id, const AttributeMap & attributes, const Lanelet & crosswalk_lanelet,
+    const Polygon3d & crosswalk_area, const LineStrings3d & stop_line)
+  {
+    return Ptr{new Crosswalk(id, attributes, crosswalk_lanelet, crosswalk_area, stop_line)};
+  }
+
+  /**
+   * @brief get the relevant crosswalk area
+   * @return crosswalk area
+   */
+  [[nodiscard]] ConstPolygons3d crosswalkAreas() const;
+
+  /**
+   * @brief get the relevant crosswalk line
+   * @return stop line
+   */
+  [[nodiscard]] ConstLineStrings3d stopLines() const;
+
+  /**
+   * @brief get the relevant crosswalk lanelet
+   * @return lanelet
+   */
+  [[nodiscard]] ConstLanelet crosswalkLanelet() const;
+
+  /**
+   * @brief add a new crosswalk area
+   * @param primitive crosswalk area to add
+   */
+  void addCrosswalkArea(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a crosswalk area
+   * @param primitive the primitive
+   * @return true if the crosswalk area existed and was removed
+   */
+  bool removeCrosswalkArea(const Polygon3d & primitive);
+
+private:
+  Crosswalk(
+    Id id, const AttributeMap & attributes, const Lanelet & crosswalk_lanelet,
+    const Polygon3d & crosswalk_area, const LineStrings3d & stop_line);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<Crosswalk>;
+  explicit Crosswalk(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__CROSSWALK_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp
new file mode 100644
index 0000000..698ab38
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp
@@ -0,0 +1,96 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class DetectionArea : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<DetectionArea>;
+  static constexpr char RuleName[] = "detection_area";
+
+  //! Directly construct a stop line from its required rule parameters.
+  //! Might modify the input data in oder to get correct tags.
+  static Ptr make(
+    Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas,
+    const LineString3d & stopLine)
+  {
+    return Ptr{new DetectionArea(id, attributes, detectionAreas, stopLine)};
+  }
+
+  /**
+   * @brief get the relevant detection_areas
+   * @return detection_areas
+   */
+  [[nodiscard]] ConstPolygons3d detectionAreas() const;
+  [[nodiscard]] Polygons3d detectionAreas();
+
+  /**
+   * @brief add a new detection area
+   * @param primitive detection area to add
+   */
+  void addDetectionArea(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a detection area
+   * @param primitive the primitive
+   * @return true if the detection area existed and was removed
+   */
+  bool removeDetectionArea(const Polygon3d & primitive);
+
+  /**
+   * @brief get the stop line for the detection area
+   * @return the stop line as LineString
+   */
+  [[nodiscard]] ConstLineString3d stopLine() const;
+  [[nodiscard]] LineString3d stopLine();
+
+  /**
+   * @brief set a new stop line, overwrite the old one
+   * @param stopLine new stop line
+   */
+  void setStopLine(const LineString3d & stopLine);
+
+  //! Deletes the stop line
+  void removeStopLine();
+
+private:
+  DetectionArea(
+    Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas,
+    const LineString3d & stopLine);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<DetectionArea>;
+  explicit DetectionArea(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__DETECTION_AREA_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp
new file mode 100644
index 0000000..8ef43fa
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp
@@ -0,0 +1,72 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_PARKING_AREA_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_PARKING_AREA_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class NoParkingArea : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<NoParkingArea>;
+  static constexpr char RuleName[] = "no_parking_area";
+
+  static Ptr make(Id id, const AttributeMap & attributes, const Polygons3d & no_parking_areas)
+  {
+    return Ptr{new NoParkingArea(id, attributes, no_parking_areas)};
+  }
+
+  /**
+   * @brief get the relevant no parking area
+   * @return no parking area
+   */
+  [[nodiscard]] ConstPolygons3d noParkingAreas() const;
+  [[nodiscard]] Polygons3d noParkingAreas();
+
+  /**
+   * @brief add a new no parking area
+   * @param primitive no parking area to add
+   */
+  void addNoParkingArea(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a no parking area
+   * @param primitive the primitive
+   * @return true if the no parking area existed and was removed
+   */
+  bool removeNoParkingArea(const Polygon3d & primitive);
+
+private:
+  NoParkingArea(Id id, const AttributeMap & attributes, const Polygons3d & no_parking_area);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<NoParkingArea>;
+  explicit NoParkingArea(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_PARKING_AREA_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp
new file mode 100644
index 0000000..eaf263b
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp
@@ -0,0 +1,94 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class NoStoppingArea : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<NoStoppingArea>;
+  static constexpr char RuleName[] = "no_stopping_area";
+
+  //! Directly construct a stop line from its required rule parameters.
+  //! Might modify the input data in oder to get correct tags.
+  static Ptr make(
+    Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_areas,
+    const Optional<LineString3d> & stopLine = {})
+  {
+    return Ptr{new NoStoppingArea(id, attributes, no_stopping_areas, stopLine)};
+  }
+
+  /**
+   * @brief get the relevant no stopping area
+   * @return no stopping area
+   */
+  [[nodiscard]] ConstPolygons3d noStoppingAreas() const;
+  [[nodiscard]] Polygons3d noStoppingAreas();
+
+  /**
+   * @brief add a new no stopping area
+   * @param primitive no stopping area to add
+   */
+  void addNoStoppingArea(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a no stopping area
+   * @param primitive the primitive
+   * @return true if the no stopping area existed and was removed
+   */
+  bool removeNoStoppingArea(const Polygon3d & primitive);
+
+  /**
+   * @brief get the stop line for the no stopping area
+   * @return the stop line as LineString
+   */
+  [[nodiscard]] Optional<ConstLineString3d> stopLine() const;
+  [[nodiscard]] Optional<LineString3d> stopLine();
+
+  /**
+   * @brief set a new stop line, overwrite the old one
+   * @param stopLine new stop line
+   */
+  void setStopLine(const LineString3d & stopLine);
+
+  //! Deletes the stop line
+  void removeStopLine();
+
+private:
+  NoStoppingArea(
+    Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_area,
+    const Optional<LineString3d> & stopLine);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<NoStoppingArea>;
+  explicit NoStoppingArea(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__NO_STOPPING_AREA_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp
new file mode 100644
index 0000000..0fc4142
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp
@@ -0,0 +1,72 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/LineString.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class RoadMarking : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<RoadMarking>;
+  static constexpr char RuleName[] = "road_marking";
+
+  //! Directly construct a stop line from its required rule parameters.
+  //! Might modify the input data in oder to get correct tags.
+  static Ptr make(Id id, const AttributeMap & attributes, const LineString3d & road_marking)
+  {
+    return Ptr{new RoadMarking(id, attributes, road_marking)};
+  }
+
+  /**
+   * @brief get the relevant road marking
+   * @return road marking
+   */
+  [[nodiscard]] ConstLineString3d roadMarking() const;
+  [[nodiscard]] LineString3d roadMarking();
+
+  /**
+   * @brief add a new road marking
+   * @param primitive road marking to add
+   */
+  void setRoadMarking(const LineString3d & road_marking);
+
+  /**
+   * @brief remove a road marking
+   */
+  void removeRoadMarking();
+
+private:
+  RoadMarking(Id id, const AttributeMap & attributes, const LineString3d & road_marking);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<RoadMarking>;
+  explicit RoadMarking(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__ROAD_MARKING_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/speed_bump.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/speed_bump.hpp
new file mode 100644
index 0000000..59c8d55
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/speed_bump.hpp
@@ -0,0 +1,74 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome, Mehmet Dogru
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__SPEED_BUMP_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__SPEED_BUMP_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class SpeedBump : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<SpeedBump>;
+  static constexpr char RuleName[] = "speed_bump";
+
+  static Ptr make(Id id, const AttributeMap & attributes, const Polygon3d & speed_bump)
+  {
+    return Ptr{new SpeedBump(id, attributes, speed_bump)};
+  }
+
+  /**
+   * @brief get the relevant speed bumps
+   * @return speed bumps
+   */
+  [[nodiscard]] ConstPolygon3d speedBump() const;
+  [[nodiscard]] Polygon3d speedBump();
+
+  /**
+   * @brief add a new speed bump
+   * @param primitive speed bump to add
+   */
+  void addSpeedBump(const Polygon3d & primitive);
+
+  /**
+   * @brief remove a speed bump
+   * @param primitive speed bump to remove
+   * @return true if the speed bump existed and was removed
+   */
+  bool removeSpeedBump(const Polygon3d & primitive);
+
+private:
+  SpeedBump(Id id, const AttributeMap & attributes, const Polygon3d & speed_bump);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<SpeedBump>;
+  explicit SpeedBump(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__SPEED_BUMP_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp
new file mode 100644
index 0000000..fdbb633
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp
@@ -0,0 +1,79 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_
+#define LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
+
+#include <lanelet2_core/primitives/LineString.h>
+
+#include <memory>
+
+namespace lanelet::autoware
+{
+class VirtualTrafficLight : public lanelet::RegulatoryElement
+{
+public:
+  using Ptr = std::shared_ptr<VirtualTrafficLight>;
+  using ConstPtr = std::shared_ptr<const VirtualTrafficLight>;
+  static constexpr char RuleName[] = "virtual_traffic_light";
+
+  static Ptr make(
+    const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light)
+  {
+    return Ptr{new VirtualTrafficLight(id, attributes, virtual_traffic_light)};
+  }
+
+  [[nodiscard]] ConstLineString3d getVirtualTrafficLight() const
+  {
+    return getParameters<ConstLineString3d>(RoleName::Refers).front();
+  }
+
+  [[nodiscard]] Optional<ConstLineString3d> getStopLine() const
+  {
+    const auto vec = getParameters<ConstLineString3d>(RoleName::RefLine);
+    if (vec.empty()) {
+      return {};
+    }
+    return vec.front();
+  }
+
+  [[nodiscard]] ConstLineString3d getStartLine() const
+  {
+    return getParameters<ConstLineString3d>("start_line").front();
+  }
+
+  [[nodiscard]] ConstLineStrings3d getEndLines() const
+  {
+    return getParameters<ConstLineString3d>("end_line");
+  }
+
+private:
+  VirtualTrafficLight(
+    const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light);
+
+  // the following lines are required so that lanelet2 can create this object
+  // when loading a map with this regulatory element
+  friend class RegisterRegulatoryElement<VirtualTrafficLight>;
+  explicit VirtualTrafficLight(const lanelet::RegulatoryElementDataPtr & data);
+};
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__REGULATORY_ELEMENTS__VIRTUAL_TRAFFIC_LIGHT_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp b/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp
new file mode 100644
index 0000000..f88e5ce
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/utility/message_conversion.hpp
@@ -0,0 +1,98 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_
+#define LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/point32.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_routing/Forward.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+namespace lanelet::utils::conversion
+{
+/**
+ * [toBinMsg converts lanelet2 map to ROS message. Similar implementation to
+ * lanelet::io_handlers::BinHandler::write()]
+ * @param map [lanelet map data]
+ * @param msg [converted ROS message. Only "data" field is filled]
+ */
+void toBinMsg(const lanelet::LaneletMapPtr & map, autoware_auto_mapping_msgs::msg::HADMapBin * msg);
+
+/**
+ * [fromBinMsg converts ROS message into lanelet2 data. Similar implementation
+ * to lanelet::io_handlers::BinHandler::parse()]
+ * @param msg [ROS message for lanelet map]
+ * @param map [Converted lanelet2 data]
+ */
+void fromBinMsg(const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map);
+void fromBinMsg(
+  const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map,
+  lanelet::traffic_rules::TrafficRulesPtr * traffic_rules,
+  lanelet::routing::RoutingGraphPtr * routing_graph);
+
+/**
+ * [toGeomMsgPt converts various point types to geometry_msgs point]
+ * @param src [input point(geometry_msgs::msg::Point3,
+ * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ]
+ * @param dst [converted geometry_msgs point]
+ */
+void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst);
+void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst);
+void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst);
+void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst);
+
+/**
+ * [toGeomMsgPt converts various point types to geometry_msgs point]
+ * @param src [input point(geometry_msgs::msg::Point3,
+ * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ]
+ * @return    [converted geometry_msgs point]
+ */
+geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src);
+geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src);
+geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src);
+geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src);
+
+lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src);
+void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst);
+
+/**
+ * [toGeomMsgPoly converts lanelet polygon to geometry_msgs polygon]
+ * @param ll_poly   [input polygon]
+ * @param geom_poly [converted geometry_msgs point]
+ */
+void toGeomMsgPoly(
+  const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly);
+
+/**
+ * [toGeomMsgPt32 converts Eigen::Vector3d(lanelet:BasicPoint3d to
+ * geometry_msgs::msg::Point32)]
+ * @param src [input point]
+ * @param dst [converted point]
+ */
+void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst);
+
+}  // namespace lanelet::utils::conversion
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__UTILITY__MESSAGE_CONVERSION_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/lanelet2_extension/include/lanelet2_extension/utility/query.hpp
new file mode 100644
index 0000000..b094a2b
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/utility/query.hpp
@@ -0,0 +1,301 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__UTILITY__QUERY_HPP_
+#define LANELET2_EXTENSION__UTILITY__QUERY_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+#include "lanelet2_extension/regulatory_elements/crosswalk.hpp"
+#include "lanelet2_extension/regulatory_elements/detection_area.hpp"
+#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp"
+#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
+#include "lanelet2_extension/regulatory_elements/speed_bump.hpp"
+
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_routing/RoutingGraph.h>
+
+#include <limits>
+#include <string>
+#include <vector>
+
+namespace lanelet::utils::query
+{
+/**
+ * [laneletLayer converts laneletLayer into lanelet vector]
+ * @param  ll_Map [input lanelet map]
+ * @return        [all lanelets in the map]
+ */
+lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapConstPtr & ll_Map);
+
+/**
+ * [subtypeLanelets extracts Lanelet that has given subtype attribute]
+ * @param  lls     [input lanelets with various subtypes]
+ * @param  subtype [subtype of lanelets to be retrieved (e.g.
+ * lanelet::AttributeValueString::Road)]
+ * @return         [lanelets with given subtype]
+ */
+lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets & lls, const char subtype[]);
+
+/**
+ * [crosswalkLanelets extracts crosswalk lanelets]
+ * @param  lls [input lanelets with various subtypes]
+ * @return     [crosswalk lanelets]
+ */
+lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets & lls);
+lanelet::ConstLanelets walkwayLanelets(const lanelet::ConstLanelets & lls);
+
+/**
+ * [roadLanelets extracts road lanelets]
+ * @param  lls [input lanelets with subtype road]
+ * @return     [road lanelets]
+ */
+lanelet::ConstLanelets roadLanelets(const lanelet::ConstLanelets & lls);
+
+/**
+ * [shoulderLanelets extracts shoulder lanelets]
+ * @param  lls [input lanelets with subtype shoulder]
+ * @return     [shoulder lanelets]
+ */
+lanelet::ConstLanelets shoulderLanelets(const lanelet::ConstLanelets & lls);
+/**
+ * [trafficLights extracts Traffic Light regulatory element from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [traffic light that are associated with input lanelets]
+ */
+std::vector<lanelet::TrafficLightConstPtr> trafficLights(const lanelet::ConstLanelets & lanelets);
+
+/**
+ * [autowareTrafficLights extracts Autoware Traffic Light regulatory element
+ * from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [autoware traffic light that are associated with input
+ * lanelets]
+ */
+std::vector<lanelet::AutowareTrafficLightConstPtr> autowareTrafficLights(
+  const lanelet::ConstLanelets & lanelets);
+
+/**
+ * [detectionAreas extracts Detection Area regulatory elements from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [detection areas that are associated with input lanelets]
+ */
+std::vector<lanelet::DetectionAreaConstPtr> detectionAreas(const lanelet::ConstLanelets & lanelets);
+
+/**
+ * [noStoppingArea extracts NoStopping Area regulatory elements from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [no stopping areas that are associated with input lanelets]
+ */
+std::vector<lanelet::NoStoppingAreaConstPtr> noStoppingAreas(
+  const lanelet::ConstLanelets & lanelets);
+
+/**
+ * [noParkingArea extracts NoParking Area regulatory elements from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [no parking areas that are associated with input lanelets]
+ */
+std::vector<lanelet::NoParkingAreaConstPtr> noParkingAreas(const lanelet::ConstLanelets & lanelets);
+
+/**
+ * [speedBumps extracts Speed Bump regulatory elements from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [speed bumps that are associated with input lanelets]
+ */
+std::vector<lanelet::SpeedBumpConstPtr> speedBumps(const lanelet::ConstLanelets & lanelets);
+
+/**
+ * [crosswalks extracts Crosswalk regulatory elements from lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [crosswalks that are associated with input lanelets]
+ */
+std::vector<lanelet::CrosswalkConstPtr> crosswalks(const lanelet::ConstLanelets & lanelets);
+
+// query all curbstones in lanelet2 map
+lanelet::ConstLineStrings3d curbstones(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query all polygons that has given type in lanelet2 map
+lanelet::ConstPolygons3d getAllPolygonsByType(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type);
+
+// query all obstacle polygons in lanelet2 map
+lanelet::ConstPolygons3d getAllObstaclePolygons(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query all parking lots in lanelet2 map
+lanelet::ConstPolygons3d getAllParkingLots(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query all partitions in lanelet2 map
+lanelet::ConstLineStrings3d getAllPartitions(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query all fences in lanelet2 map
+lanelet::ConstLineStrings3d getAllFences(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query all pedestrian polygon markings in lanelet2 map
+lanelet::ConstLineStrings3d getAllPedestrianPolygonMarkings(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query all pedestrian line markings in lanelet2 map
+lanelet::ConstLineStrings3d getAllPedestrianLineMarkings(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query all parking spaces in lanelet2 map
+lanelet::ConstLineStrings3d getAllParkingSpaces(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// query linked parking spaces from lanelet
+lanelet::ConstLineStrings3d getLinkedParkingSpaces(
+  const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+lanelet::ConstLineStrings3d getLinkedParkingSpaces(
+  const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces,
+  const lanelet::ConstPolygons3d & all_parking_lots);
+// query linked lanelets from parking space
+bool getLinkedLanelet(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstLanelets & all_road_lanelets,
+  const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet);
+bool getLinkedLanelet(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet);
+lanelet::ConstLanelets getLinkedLanelets(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstLanelets & all_road_lanelets,
+  const lanelet::ConstPolygons3d & all_parking_lots);
+lanelet::ConstLanelets getLinkedLanelets(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
+// get linked parking lot from lanelet
+bool getLinkedParkingLot(
+  const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots,
+  lanelet::ConstPolygon3d * linked_parking_lot);
+// get linked parking lot from current pose of ego car
+bool getLinkedParkingLot(
+  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots,
+  lanelet::ConstPolygon3d * linked_parking_lot);
+// get linked parking lot from parking space
+bool getLinkedParkingLot(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot);
+
+// query linked parking space from parking lot
+lanelet::ConstLineStrings3d getLinkedParkingSpaces(
+  const lanelet::ConstPolygon3d & parking_lot,
+  const lanelet::ConstLineStrings3d & all_parking_spaces);
+// query linked lanelets from parking lot
+lanelet::ConstLanelets getLinkedLanelets(
+  const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets);
+
+/**
+ * [stopLinesLanelets extracts stoplines that are associated to lanelets]
+ * @param lanelets [input lanelets]
+ * @return         [stop lines that are associated with input lanelets]
+ */
+std::vector<lanelet::ConstLineString3d> stopLinesLanelets(const lanelet::ConstLanelets & lanelets);
+
+/**
+ * [stopLinesLanelet extracts stop lines that are associated with a given
+ * lanelet]
+ * @param ll [input lanelet]
+ * @return   [stop lines that are associated with input lanelet]
+ */
+std::vector<lanelet::ConstLineString3d> stopLinesLanelet(const lanelet::ConstLanelet & ll);
+
+/**
+ * [stopSignStopLines extracts stoplines that are associated with stop signs]
+ * @param lanelets     [input lanelets]
+ * @param stop_sign_id [sign id of stop sign]
+ * @return             [array of stoplines]
+ */
+std::vector<lanelet::ConstLineString3d> stopSignStopLines(
+  const lanelet::ConstLanelets & lanelets, const std::string & stop_sign_id = "stop_sign");
+
+ConstLanelets getLaneletsWithinRange(
+  const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point,
+  const double range);
+ConstLanelets getLaneletsWithinRange(
+  const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point,
+  const double range);
+
+ConstLanelets getLaneChangeableNeighbors(
+  const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet);
+ConstLanelets getLaneChangeableNeighbors(
+  const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets,
+  const geometry_msgs::msg::Point & search_point);
+
+ConstLanelets getAllNeighbors(const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet);
+ConstLanelets getAllNeighborsLeft(
+  const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet);
+ConstLanelets getAllNeighborsRight(
+  const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet);
+ConstLanelets getAllNeighbors(
+  const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets,
+  const geometry_msgs::msg::Point & search_point);
+
+bool getClosestLanelet(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelet * closest_lanelet_ptr);
+
+bool getClosestLaneletWithConstrains(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelet * closest_lanelet_ptr,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+
+bool getCurrentLanelets(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point,
+  ConstLanelets * current_lanelets_ptr);
+
+bool getCurrentLanelets(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelets * current_lanelets_ptr);
+
+/**
+ * [getSucceedingLaneletSequences retrieves a sequence of lanelets after the given lanelet.
+ * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence
+ * does not include input lanelet.]
+ * @param graph [input lanelet routing graph]
+ * @param lanelet [input lanelet]
+ * @param length [minimum length of retrieved lanelet sequence]
+ * @return   [lanelet sequence that follows given lanelet]
+ */
+std::vector<lanelet::ConstLanelets> getSucceedingLaneletSequences(
+  const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length);
+
+/**
+ * [getPrecedingLaneletSequences retrieves a sequence of lanelets before the given lanelet.
+ * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence
+ * does not include input lanelet.]
+ * @param graph [input lanelet routing graph]
+ * @param lanelet [input lanelet]
+ * @param length [minimum length of retrieved lanelet sequence]
+ * @return   [lanelet sequence that leads to given lanelet]
+ */
+std::vector<lanelet::ConstLanelets> getPrecedingLaneletSequences(
+  const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length, const lanelet::ConstLanelets & exclude_lanelets = {});
+
+}  // namespace lanelet::utils::query
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__UTILITY__QUERY_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp b/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp
new file mode 100644
index 0000000..5d4e7b7
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/utility/route_checker.hpp
@@ -0,0 +1,33 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_
+#define LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+
+#include <lanelet2_core/Forward.h>
+
+namespace lanelet::utils::route
+{
+using autoware_planning_msgs::msg::LaneletRoute;
+
+bool isRouteValid(const LaneletRoute & route, const lanelet::LaneletMapPtr lanelet_map_ptr_);
+}  // namespace lanelet::utils::route
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__UTILITY__ROUTE_CHECKER_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp
new file mode 100644
index 0000000..7df3f6a
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp
@@ -0,0 +1,99 @@
+// Copyright 2015-2023 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Kenji Miyake, Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_
+#define LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/Point.h>
+#include <lanelet2_routing/Forward.h>
+
+#include <map>
+
+namespace lanelet::utils
+{
+
+// @brief combine multiple lanelets into one, solely focusing on the shape and discarding any
+// associated information such as ID and attributes. InvalId is set for the ID.
+// @param lanelets to be combined.
+lanelet::ConstLanelet combineLaneletsShape(const lanelet::ConstLanelets & lanelets);
+
+lanelet::LineString3d generateFineCenterline(
+  const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0);
+lanelet::ConstLineString3d getCenterlineWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0);
+lanelet::ConstLineString3d getRightBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0);
+lanelet::ConstLineString3d getLeftBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0);
+
+lanelet::ConstLanelet getExpandedLanelet(
+  const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset);
+
+lanelet::ConstLanelets getExpandedLanelets(
+  const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset);
+
+/**
+ * @brief  Apply a patch for centerline because the original implementation
+ * doesn't have enough quality
+ */
+void overwriteLaneletsCenterline(
+  lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0,
+  const bool force_overwrite = false);
+
+lanelet::ConstLanelets getConflictingLanelets(
+  const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet);
+
+bool lineStringWithWidthToPolygon(
+  const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon);
+
+bool lineStringToPolygon(
+  const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon);
+
+double getLaneletLength2d(const lanelet::ConstLanelet & lanelet);
+double getLaneletLength3d(const lanelet::ConstLanelet & lanelet);
+double getLaneletLength2d(const lanelet::ConstLanelets & lanelet_sequence);
+double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence);
+
+lanelet::ArcCoordinates getArcCoordinates(
+  const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose);
+
+lanelet::ConstLineString3d getClosestSegment(
+  const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring);
+
+lanelet::CompoundPolygon3d getPolygonFromArcLength(
+  const lanelet::ConstLanelets & lanelets, const double s1, const double s2);
+double getLaneletAngle(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point);
+bool isInLanelet(
+  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
+  const double radius = 0.0);
+geometry_msgs::msg::Pose getClosestCenterPose(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point);
+double getLateralDistanceToCenterline(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & pose);
+double getLateralDistanceToClosestLanelet(
+  const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose);
+}  // namespace lanelet::utils
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__UTILITY__UTILITIES_HPP_
diff --git a/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
new file mode 100644
index 0000000..c904c43
--- /dev/null
+++ b/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
@@ -0,0 +1,371 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+#ifndef LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_
+#define LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp"
+#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
+#include "lanelet2_extension/utility/query.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/polygon.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <string>
+#include <vector>
+
+namespace lanelet::visualization
+{
+/**
+ * [lanelet2Triangle converts lanelet into vector of triangles. Used for
+ * triangulation]
+ * @param ll        [input lanelet]
+ * @param triangles [array of polygon message, each containing 3 vertices]
+ */
+void lanelet2Triangle(
+  const lanelet::ConstLanelet & ll, std::vector<geometry_msgs::msg::Polygon> * triangles);
+
+/**
+ * [polygon2Triangle converts polygon into vector of triangles]
+ * @param polygon   [input polygon]
+ * @param triangles [array of polygon message, each containing 3 vertices]
+ */
+void polygon2Triangle(
+  const geometry_msgs::msg::Polygon & polygon,
+  std::vector<geometry_msgs::msg::Polygon> * triangles);
+
+/**
+ * [lanelet2Polygon converts lanelet into a polygon]
+ * @param ll      [input lanelet]
+ * @param polygon [polygon message containing shape of the input lanelet.]
+ */
+void lanelet2Polygon(const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon);
+
+/**
+ * [initLineStringMarker initializes marker to visualize shape of linestring]
+ * @param marker     [output marker message]
+ * @param frame_id   [frame id of the marker]
+ * @param ns         [namespace of the marker]
+ * @param c          [color of the marker]
+ */
+void initLineStringMarker(
+  visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [pushLineStringMarker pushes marker vertices to visualize shape of linestring]
+ * @param marker     [output marker message]
+ * @param ls         [input linestring]
+ * @param c          [color of the marker]
+ * @param lss        [thickness of the marker]
+ */
+void pushLineStringMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
+  const std_msgs::msg::ColorRGBA & c, const float lss = 0.1);
+
+/**
+ * [initArrowsMarker initializes marker to visualize arrows with TRIANGLE_LIST]
+ * @param marker     [output marker message]
+ * @param frame_id   [frame id of the marker]
+ * @param ns         [namespace of the marker]
+ * @param c          [color of the marker]
+ */
+void initArrowsMarker(
+  visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [pushArrowsMarker pushes marker to visualize arrows with TRIANGLE_LIST]
+ * @param marker     [output marker message]
+ * @param ls         [input linestring]
+ * @param c          [color of the marker]
+ */
+void pushArrowsMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic
+ * lights]
+ * @param marker   [created marker]
+ * @param ns       [namespace of the marker]
+ * @param duration [lifetime of the marker]
+ */
+void initTrafficLightTriangleMarker(
+  visualization_msgs::msg::Marker * marker, const std::string & ns,
+  const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
+
+/**
+ * [pushTrafficLightTriangleMarker pushes marker vertices to visualize shape of traffic
+ * lights]
+ * @param marker   [created marker]
+ * @param ls       [linestring that represents traffic light shape]
+ * @param cl       [color of the marker]
+ * @param scale    [scale of the marker]
+ */
+void pushTrafficLightTriangleMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
+  const std_msgs::msg::ColorRGBA & cl, const double scale = 1.0);
+
+/**
+ * [laneletsBoundaryAsMarkerArray create marker array to visualize shape of
+ * boundaries of lanelets]
+ * @param  lanelets       [input lanelets]
+ * @param  c              [color of the boundary]
+ * @param  viz_centerline [flag to visualize centerline or not]
+ * @return                [created marker array]
+ */
+visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray(
+  const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c,
+  const bool viz_centerline, const std::string & additional_namespace = "");
+/**
+ * [laneletsAsTriangleMarkerArray create marker array to visualize shape of the
+ * lanelet]
+ * @param  ns       [namespace of the marker]
+ * @param  lanelets [input lanelets]
+ * @param  c        [color of the marker]
+ * @return          [created marker]
+ */
+visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray(
+  const std::string & ns, const lanelet::ConstLanelets & lanelets,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [laneletDirectionAsMarkerArray create marker array to visualize direction of
+ * the lanelet]
+ * @param  lanelets [input lanelets]
+ * @return          [created marker array]
+ */
+visualization_msgs::msg::MarkerArray laneletDirectionAsMarkerArray(
+  const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace = "");
+
+/**
+ * [lineStringsAsMarkerArray creates marker array to visualize shape of
+ * linestrings]
+ * @param  line_strings [input linestrings]
+ * @param  name_space   [namespace of the marker]
+ * @param  c            [color of the marker]
+ * @param  lss          [thickness of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray(
+  const std::vector<lanelet::ConstLineString3d> & line_strings, const std::string & name_space,
+  const std_msgs::msg::ColorRGBA & c, const float lss);
+
+/**
+ * [autowareTrafficLightsAsMarkerArray creates marker array to visualize traffic
+ * lights]
+ * @param  tl_reg_elems [traffic light regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray(
+  const std::vector<lanelet::AutowareTrafficLightConstPtr> & tl_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0),
+  const double scale = 1.0);
+
+/**
+ * [generateTrafficLightRegulatoryElementIdMaker creates marker array to visualize traffic
+ * light regulatory element ids]
+ * @param lanelets      [lanelets]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::msg::MarkerArray generateTrafficLightRegulatoryElementIdMaker(
+  const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration = rclcpp::Duration(0, 0), const double scale = 0.5);
+
+/**
+ * [generateTrafficLightIdMarkerArray creates marker array to visualize traffic
+ * light ids]
+ * @param  tl_reg_elems [traffic light regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::msg::MarkerArray generateTrafficLightIdMaker(
+  const std::vector<lanelet::AutowareTrafficLightConstPtr> & tl_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0),
+  const double scale = 0.5);
+
+/**
+ * [trafficLightsAsTriangleMarkerArray creates marker array to visualize shape
+ * of traffic lights]
+ * @param  tl_reg_elems [traffic light regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::msg::MarkerArray trafficLightsAsTriangleMarkerArray(
+  const std::vector<lanelet::TrafficLightConstPtr> & tl_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0),
+  const double scale = 1.0);
+
+/**
+ * [detectionAreasAsMarkerArray creates marker array to visualize detection areas]
+ * @param  da_reg_elems [detection area regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ */
+visualization_msgs::msg::MarkerArray detectionAreasAsMarkerArray(
+  const std::vector<lanelet::DetectionAreaConstPtr> & da_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
+
+/**
+ * [noParkingAreasAsMarkerArray creates marker array to visualize detection areas]
+ * @param  no_reg_elems [no parking area regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ */
+visualization_msgs::msg::MarkerArray noParkingAreasAsMarkerArray(
+  const std::vector<lanelet::NoParkingAreaConstPtr> & no_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
+
+/**
+ * [noStoppingAreasAsMarkerArray creates marker array to visualize detection areas]
+ * @param  no_reg_elems [mp stopping area regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ */
+visualization_msgs::msg::MarkerArray noStoppingAreasAsMarkerArray(
+  const std::vector<lanelet::NoStoppingAreaConstPtr> & no_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
+
+/**
+ * [speedBumpsAsMarkerArray creates marker array to visualize speed bumps]
+ * @param  sb_reg_elems [speed bump regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ */
+visualization_msgs::msg::MarkerArray speedBumpsAsMarkerArray(
+  const std::vector<lanelet::SpeedBumpConstPtr> & sb_reg_elems, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
+
+/**
+ * [crosswalkAreasAsMarkerArray creates marker array to visualize crosswalk regulatory element]
+ * @param  sb_reg_elems [crosswalk regulatory elements]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ */
+visualization_msgs::msg::MarkerArray crosswalkAreasAsMarkerArray(
+  const std::vector<lanelet::CrosswalkConstPtr> & cw_reg_elems, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration = rclcpp::Duration(0, 0));
+
+/**
+ * [pedestrianPolygonMarkingsAsMarkerArray creates marker array to visualize pedestrian polygon
+ * markings]
+ * @param  pedestrian_polygon_markings [pedestrian marking polygon]
+ * @param  c            [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray pedestrianPolygonMarkingsAsMarkerArray(
+  const lanelet::ConstLineStrings3d & pedestrian_polygon_markings,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [pedestrianLineMarkingsAsMarkerArray creates marker array to visualize pedestrian line markings]
+ * @param  pedestrian_line_markings [pedestrian marking line]
+ * @param  c            [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray pedestrianLineMarkingsAsMarkerArray(
+  const lanelet::ConstLineStrings3d & pedestrian_line_markings, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [parkingLotsAsMarkerArray creates marker array to visualize parking lots]
+ * @param  parking_lots [parking lot polygon]
+ * @param  c            [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray parkingLotsAsMarkerArray(
+  const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [parkingSpacesAsMarkerArray creates marker array to visualize parking spaces]
+ * @param  parking_spaces [parking space line string]
+ * @param  c            [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray parkingSpacesAsMarkerArray(
+  const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [detectionAreasAsMarkerArray creates marker array to visualize lanelet_id]
+ * @param road_lanelets [road lanelets]
+ * @param c [color of the marker]
+ * @param duration [lifetime of the marker]
+ * @param ns [namespace of the marker]
+ * @param scale [scale of the marker]
+ * @return visualization_msgs::msg::MarkerArray
+ */
+visualization_msgs::msg::MarkerArray generateLaneletIdMarker(
+  const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c,
+  const std::string & ns = "lanelet_id", const double scale = 0.5);
+
+visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray(
+  const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [intersectionAreaAsMarkerArray creates marker array to visualize intersection area]
+ * @param  intersection_areas [intersection area polygon]
+ * @param  c                  [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray intersectionAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & intersection_areas, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [noObstacleSegmentationAreaAsMarkerArray creates marker array to visualize no obstacle
+ * segmentation area]
+ * @param  no_obstacle_segmentation_area [no obstacle segmentation area polygon]
+ * @param  c                             [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_obstacle_segmentation_area,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [noObstacleSegmentationAreaForRunOutAsMarkerArray creates marker array to visualize no obstacle
+ * segmentation area for run out]
+ * @param  no_obstacle_segmentation_area_for_run_out [no obstacle segmentation area for run out
+ * polygon]
+ * @param  c                                         [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaForRunOutAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_obstacle_segmentation_area_for_run_out,
+  const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [hatchedRoadMarkingsAreaAsMarkerArray creates marker array to visualize hatched road markings
+ * area]
+ * @param  hatched_road_markings_area [hatched road markings area polygon]
+ * @param  area_color                 [color of the area marker]
+ * @param  line_color                 [color of the line marker]
+ */
+visualization_msgs::msg::MarkerArray hatchedRoadMarkingsAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & hatched_road_markings_area,
+  const std_msgs::msg::ColorRGBA & area_color, const std_msgs::msg::ColorRGBA & line_color);
+
+}  // namespace lanelet::visualization
+
+// NOLINTEND(readability-identifier-naming)
+
+#endif  // LANELET2_EXTENSION__VISUALIZATION__VISUALIZATION_HPP_
diff --git a/lanelet2_extension/lib/autoware_osm_parser.cpp b/lanelet2_extension/lib/autoware_osm_parser.cpp
new file mode 100644
index 0000000..5c27200
--- /dev/null
+++ b/lanelet2_extension/lib/autoware_osm_parser.cpp
@@ -0,0 +1,80 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/io/autoware_osm_parser.hpp"
+
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_io/io_handlers/Factory.h>
+#include <lanelet2_io/io_handlers/OsmFile.h>
+#include <lanelet2_io/io_handlers/OsmHandler.h>
+
+#include <memory>
+#include <string>
+
+namespace lanelet::io_handlers
+{
+std::unique_ptr<LaneletMap> AutowareOsmParser::parse(
+  const std::string & filename, ErrorMessages & errors) const
+{
+  auto map = OsmParser::parse(filename, errors);
+
+  // rerun align function in just in case
+  for (Lanelet & lanelet : map->laneletLayer) {
+    LineString3d new_left;
+    LineString3d new_right;
+    std::tie(new_left, new_right) = geometry::align(lanelet.leftBound(), lanelet.rightBound());
+    lanelet.setLeftBound(new_left);
+    lanelet.setRightBound(new_right);
+  }
+
+  return map;
+}
+
+namespace
+{
+RegisterParser<AutowareOsmParser> regParser;
+}  // namespace
+
+void AutowareOsmParser::parseVersions(
+  const std::string & filename, std::string * format_version, std::string * map_version)
+{
+  if (format_version == nullptr || map_version == nullptr) {
+    std::cerr << __FUNCTION__ << ": either format_version or map_version is null pointer!";
+    return;
+  }
+
+  pugi::xml_document doc;
+  auto result = doc.load_file(filename.c_str());
+  if (!result) {
+    throw lanelet::ParseError(
+      std::string("Errors occurred while parsing osm file: ") + result.description());
+  }
+
+  auto osmNode = doc.child("osm");
+  auto metainfo = osmNode.child("MetaInfo");
+  if (metainfo.attribute("format_version")) {
+    *format_version = metainfo.attribute("format_version").value();
+  }
+  if (metainfo.attribute("map_version")) {
+    *map_version = metainfo.attribute("map_version").value();
+  }
+}
+
+}  // namespace lanelet::io_handlers
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/autoware_traffic_light.cpp b/lanelet2_extension/lib/autoware_traffic_light.cpp
new file mode 100644
index 0000000..b0ab77a
--- /dev/null
+++ b/lanelet2_extension/lib/autoware_traffic_light.cpp
@@ -0,0 +1,149 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T & primitive, RuleParameters * member)
+{
+  if (member == nullptr) {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end()) {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+template <typename T>
+RuleParameters toRuleParameters(const std::vector<T> & primitives)
+{
+  auto cast_func = [](const auto & elem) { return static_cast<RuleParameter>(elem); };
+  return utils::transform(primitives, cast_func);
+}
+
+template <>
+RuleParameters toRuleParameters(const std::vector<LineStringOrPolygon3d> & primitives)
+{
+  auto cast_func = [](const auto & elem) { return elem.asRuleParameter(); };
+  return utils::transform(primitives, cast_func);
+}
+
+LineStringsOrPolygons3d getLsOrPoly(const RuleParameterMap & paramsMap, RoleName role)
+{
+  auto params = paramsMap.find(role);
+  if (params == paramsMap.end()) {
+    return {};
+  }
+  LineStringsOrPolygons3d result;
+  for (auto & param : params->second) {
+    auto l = boost::get<LineString3d>(&param);
+    if (l != nullptr) {
+      result.push_back(*l);
+    }
+    auto p = boost::get<Polygon3d>(&param);
+    if (p != nullptr) {
+      result.push_back(*p);
+    }
+  }
+  return result;
+}
+
+[[maybe_unused]] ConstLineStringsOrPolygons3d getConstLsOrPoly(
+  const RuleParameterMap & params, RoleName role)
+{
+  auto cast_func = [](auto & lsOrPoly) {
+    return static_cast<ConstLineStringOrPolygon3d>(lsOrPoly);
+  };
+  return utils::transform(getLsOrPoly(params, role), cast_func);
+}
+
+[[maybe_unused]] RegulatoryElementDataPtr constructAutowareTrafficLightData(
+  Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights,
+  const Optional<LineString3d> & stopLine, const LineStrings3d & lightBulbs)
+{
+  RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(trafficLights)}};
+
+  if (!!stopLine) {
+    RuleParameters rule_parameters = {*stopLine};
+    rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters));
+  }
+  if (!lightBulbs.empty()) {
+    rpm.insert(std::make_pair(
+      AutowareTrafficLight::AutowareRoleNameString::LightBulbs, toRuleParameters(lightBulbs)));
+  }
+
+  auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = AttributeValueString::TrafficLight;
+  return data;
+}
+}  // namespace
+
+AutowareTrafficLight::AutowareTrafficLight(const RegulatoryElementDataPtr & data)
+: TrafficLight(data)
+{
+}
+
+AutowareTrafficLight::AutowareTrafficLight(
+  Id id, const AttributeMap & attributes, const LineStringsOrPolygons3d & trafficLights,
+  const Optional<LineString3d> & stopLine, const LineStrings3d & lightBulbs)
+: TrafficLight(id, attributes, trafficLights, stopLine)
+{
+  for (const auto & lightBulb : lightBulbs) {
+    addLightBulbs(lightBulb);
+  }
+}
+
+ConstLineStrings3d AutowareTrafficLight::lightBulbs() const
+{
+  return getParameters<ConstLineString3d>(AutowareRoleNameString::LightBulbs);
+}
+
+void AutowareTrafficLight::addLightBulbs(const LineStringOrPolygon3d & primitive)
+{
+  parameters()[AutowareRoleNameString::LightBulbs].emplace_back(primitive.asRuleParameter());
+}
+
+bool AutowareTrafficLight::removeLightBulbs(const LineStringOrPolygon3d & primitive)
+{
+  return findAndErase(
+    primitive.asRuleParameter(), &parameters().find(AutowareRoleNameString::LightBulbs)->second);
+}
+
+RegisterRegulatoryElement<AutowareTrafficLight> regAutowareTrafficLight;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/crosswalk.cpp b/lanelet2_extension/lib/crosswalk.cpp
new file mode 100644
index 0000000..19cb5ff
--- /dev/null
+++ b/lanelet2_extension/lib/crosswalk.cpp
@@ -0,0 +1,132 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/crosswalk.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T & primitive, RuleParameters * member)
+{
+  if (member == nullptr) {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end()) {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+template <typename T>
+Optional<T> tryGetFront(const std::vector<T> & vec)
+{
+  if (vec.empty()) {
+    return {};
+  }
+  return vec.front();
+}
+
+template <typename T>
+RuleParameters toRuleParameters(const std::vector<T> & primitives)
+{
+  auto cast_func = [](const auto & elem) { return static_cast<RuleParameter>(elem); };
+  return utils::transform(primitives, cast_func);
+}
+
+RegulatoryElementDataPtr constructCrosswalk(
+  Id id, const AttributeMap & attributes, const Lanelet & crosswalkLanelet,
+  const Polygon3d & crosswalkArea, const LineStrings3d & stopLine)
+{
+  RuleParameterMap rpm;
+
+  {
+    RuleParameters rule_parameters = {crosswalkArea};
+    rpm.insert(
+      std::make_pair(Crosswalk::AutowareRoleNameString::CrosswalkPolygon, rule_parameters));
+  }
+
+  {
+    RuleParameters rule_parameters = {crosswalkLanelet};
+    rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters));
+  }
+
+  for (const auto & line : stopLine) {
+    RuleParameters rule_parameters = {line};
+    rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters));
+  }
+
+  auto data = std::make_shared<RegulatoryElementData>(id, std::move(rpm), attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "crosswalk";
+  return data;
+}
+}  // namespace
+
+Crosswalk::Crosswalk(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+}
+
+Crosswalk::Crosswalk(
+  Id id, const AttributeMap & attributes, const Lanelet & crosswalk_lanelet,
+  const Polygon3d & crosswalk_area, const LineStrings3d & stop_line)
+: Crosswalk(constructCrosswalk(id, attributes, crosswalk_lanelet, crosswalk_area, stop_line))
+{
+}
+
+ConstPolygons3d Crosswalk::crosswalkAreas() const
+{
+  return getParameters<ConstPolygon3d>(AutowareRoleNameString::CrosswalkPolygon);
+}
+
+ConstLineStrings3d Crosswalk::stopLines() const
+{
+  return getParameters<ConstLineString3d>(RoleName::RefLine);
+}
+
+ConstLanelet Crosswalk::crosswalkLanelet() const
+{
+  return getParameters<ConstLanelet>(RoleName::Refers).front();
+}
+
+void Crosswalk::addCrosswalkArea(const Polygon3d & primitive)
+{
+  parameters()["crosswalk"].emplace_back(primitive);
+}
+
+bool Crosswalk::removeCrosswalkArea(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("crosswalk")->second);
+}
+
+RegisterRegulatoryElement<Crosswalk> regCrosswalk;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/detection_area.cpp b/lanelet2_extension/lib/detection_area.cpp
new file mode 100644
index 0000000..ce7fb2d
--- /dev/null
+++ b/lanelet2_extension/lib/detection_area.cpp
@@ -0,0 +1,162 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/detection_area.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T & primitive, RuleParameters * member)
+{
+  if (member == nullptr) {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end()) {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+template <typename T>
+RuleParameters toRuleParameters(const std::vector<T> & primitives)
+{
+  auto cast_func = [](const auto & elem) { return static_cast<RuleParameter>(elem); };
+  return utils::transform(primitives, cast_func);
+}
+
+// template <>
+// RuleParameters toRuleParameters(const std::vector<Polygon3d>& primitives)
+// {
+//   auto cast_func = [](const auto& elem) { return elem.asRuleParameter(); };
+//   return utils::transform(primitives, cast_func);
+// }
+
+Polygons3d getPoly(const RuleParameterMap & paramsMap, RoleName role)
+{
+  auto params = paramsMap.find(role);
+  if (params == paramsMap.end()) {
+    return {};
+  }
+
+  Polygons3d result;
+  for (auto & param : params->second) {
+    auto p = boost::get<Polygon3d>(&param);
+    if (p != nullptr) {
+      result.push_back(*p);
+    }
+  }
+  return result;
+}
+
+ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role)
+{
+  auto cast_func = [](auto & poly) { return static_cast<ConstPolygon3d>(poly); };
+  return utils::transform(getPoly(params, role), cast_func);
+}
+
+RegulatoryElementDataPtr constructDetectionAreaData(
+  Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas,
+  const LineString3d & stopLine)
+{
+  RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(detectionAreas)}};
+
+  RuleParameters rule_parameters = {stopLine};
+  rpm.insert(std::make_pair(RoleNameString::RefLine, rule_parameters));
+
+  auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "detection_area";
+  return data;
+}
+}  // namespace
+
+DetectionArea::DetectionArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getConstPoly(data->parameters, RoleName::Refers).empty()) {
+    throw InvalidInputError("No detection area defined!");
+  }
+  if (getParameters<ConstLineString3d>(RoleName::RefLine).size() != 1) {
+    throw InvalidInputError("There must be exactly one stopline defined!");
+  }
+}
+
+DetectionArea::DetectionArea(
+  Id id, const AttributeMap & attributes, const Polygons3d & detectionAreas,
+  const LineString3d & stopLine)
+: DetectionArea(constructDetectionAreaData(id, attributes, detectionAreas, stopLine))
+{
+}
+
+ConstPolygons3d DetectionArea::detectionAreas() const
+{
+  return getConstPoly(parameters(), RoleName::Refers);
+}
+Polygons3d DetectionArea::detectionAreas()
+{
+  return getPoly(parameters(), RoleName::Refers);
+}
+
+void DetectionArea::addDetectionArea(const Polygon3d & primitive)
+{
+  parameters()["detection_area"].emplace_back(primitive);
+}
+
+bool DetectionArea::removeDetectionArea(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("detection_area")->second);
+}
+
+ConstLineString3d DetectionArea::stopLine() const
+{
+  return getParameters<ConstLineString3d>(RoleName::RefLine).front();
+}
+
+LineString3d DetectionArea::stopLine()
+{
+  return getParameters<LineString3d>(RoleName::RefLine).front();
+}
+
+void DetectionArea::setStopLine(const LineString3d & stopLine)
+{
+  parameters()[RoleName::RefLine] = {stopLine};
+}
+
+void DetectionArea::removeStopLine()
+{
+  parameters()[RoleName::RefLine] = {};
+}
+
+RegisterRegulatoryElement<DetectionArea> regDetectionArea;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/landmark.cpp b/lanelet2_extension/lib/landmark.cpp
new file mode 100644
index 0000000..021103a
--- /dev/null
+++ b/lanelet2_extension/lib/landmark.cpp
@@ -0,0 +1,50 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "lanelet2_extension/localization/landmark.hpp"
+
+#include "lanelet2_extension/utility/message_conversion.hpp"
+
+#include <Eigen/Core>
+
+#include <lanelet2_core/LaneletMap.h>
+
+namespace lanelet::localization
+{
+
+std::vector<lanelet::Polygon3d> parseLandmarkPolygons(
+  const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg,
+  const std::string & target_subtype)
+{
+  lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared<lanelet::LaneletMap>()};
+  lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr);
+
+  std::vector<lanelet::Polygon3d> landmarks;
+
+  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
+    const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")};
+    if (type != "pose_marker") {
+      continue;
+    }
+    const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")};
+    if (subtype != target_subtype) {
+      continue;
+    }
+    landmarks.push_back(poly);
+  }
+
+  return landmarks;
+}
+
+}  // namespace lanelet::localization
diff --git a/lanelet2_extension/lib/message_conversion.cpp b/lanelet2_extension/lib/message_conversion.cpp
new file mode 100644
index 0000000..53878e7
--- /dev/null
+++ b/lanelet2_extension/lib/message_conversion.cpp
@@ -0,0 +1,194 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/utility/message_conversion.hpp"
+
+#include "lanelet2_extension/projection/mgrs_projector.hpp"
+
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_io/Exceptions.h>
+#include <lanelet2_io/Projection.h>
+#include <lanelet2_io/io_handlers/OsmFile.h>
+#include <lanelet2_io/io_handlers/OsmHandler.h>
+#include <lanelet2_io/io_handlers/Serialize.h>
+#include <lanelet2_projection/UTM.h>
+#include <lanelet2_routing/RoutingGraph.h>
+
+#include <sstream>
+#include <string>
+#include <utility>
+
+namespace lanelet::utils::conversion
+{
+void toBinMsg(const lanelet::LaneletMapPtr & map, autoware_auto_mapping_msgs::msg::HADMapBin * msg)
+{
+  if (msg == nullptr) {
+    std::cerr << __FUNCTION__ << "msg is null pointer!";
+    return;
+  }
+
+  std::stringstream ss;
+  boost::archive::binary_oarchive oa(ss);
+  oa << *map;
+  auto id_counter = lanelet::utils::getId();
+  oa << id_counter;
+
+  std::string data_str(ss.str());
+
+  msg->data.clear();
+  msg->data.assign(data_str.begin(), data_str.end());
+}
+
+void fromBinMsg(const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map)
+{
+  if (!map) {
+    std::cerr << __FUNCTION__ << ": map is null pointer!";
+    return;
+  }
+
+  std::string data_str;
+  data_str.assign(msg.data.begin(), msg.data.end());
+
+  std::stringstream ss;
+  ss << data_str;
+  boost::archive::binary_iarchive oa(ss);
+  oa >> *map;
+  lanelet::Id id_counter = 0;
+  oa >> id_counter;
+  lanelet::utils::registerId(id_counter);
+  // *map = std::move(laneletMap);
+}
+
+void fromBinMsg(
+  const autoware_auto_mapping_msgs::msg::HADMapBin & msg, lanelet::LaneletMapPtr map,
+  lanelet::traffic_rules::TrafficRulesPtr * traffic_rules,
+  lanelet::routing::RoutingGraphPtr * routing_graph)
+{
+  fromBinMsg(msg, map);
+  *traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+    lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  *routing_graph = lanelet::routing::RoutingGraph::build(*map, **traffic_rules);
+}
+
+void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!";
+    return;
+  }
+  dst->x = src.x;
+  dst->y = src.y;
+  dst->z = src.z;
+}
+void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!";
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = src.z();
+}
+void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!";
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = src.z();
+}
+void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!" << std::endl;
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = 0;
+}
+
+void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!" << std::endl;
+    return;
+  }
+  dst->x = static_cast<float>(src.x());
+  dst->y = static_cast<float>(src.y());
+  dst->z = static_cast<float>(src.z());
+}
+
+geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src)
+{
+  geometry_msgs::msg::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src)
+{
+  geometry_msgs::msg::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src)
+{
+  geometry_msgs::msg::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src)
+{
+  geometry_msgs::msg::Point dst;
+  toGeomMsgPt(src, &dst);
+  return dst;
+}
+
+lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src)
+{
+  lanelet::ConstPoint3d dst;
+  toLaneletPoint(src, &dst);
+  return dst;
+}
+
+void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst)
+{
+  *dst = lanelet::Point3d(lanelet::InvalId, src.x, src.y, src.z);
+}
+
+void toGeomMsgPoly(const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly)
+{
+  geom_poly->points.clear();
+  geom_poly->points.reserve(ll_poly.size());
+  for (const auto & ll_pt : ll_poly) {
+    geometry_msgs::msg::Point32 geom_pt32;
+    utils::conversion::toGeomMsgPt32(ll_pt.basicPoint(), &geom_pt32);
+    geom_poly->points.push_back(geom_pt32);
+  }
+}
+
+}  // namespace lanelet::utils::conversion
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/mgrs_projector.cpp b/lanelet2_extension/lib/mgrs_projector.cpp
new file mode 100644
index 0000000..891df1b
--- /dev/null
+++ b/lanelet2_extension/lib/mgrs_projector.cpp
@@ -0,0 +1,135 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/projection/mgrs_projector.hpp"
+
+#include <iostream>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace lanelet::projection
+{
+MGRSProjector::MGRSProjector(Origin origin) : Projector(origin)
+{
+}
+
+BasicPoint3d MGRSProjector::forward(const GPSPoint & gps) const
+{
+  BasicPoint3d mgrs_point(forward(gps, 0));
+  return mgrs_point;
+}
+
+BasicPoint3d MGRSProjector::forward(const GPSPoint & gps, const int precision) const
+{
+  std::string prev_projected_grid = projected_grid_;
+
+  BasicPoint3d mgrs_point{0., 0., gps.ele};
+  BasicPoint3d utm_point{0., 0., gps.ele};
+  int zone{};
+  bool is_north{};
+  std::string mgrs_code;
+
+  try {
+    GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, is_north, utm_point.x(), utm_point.y());
+    GeographicLib::MGRS::Forward(
+      zone, is_north, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code);
+  } catch (const GeographicLib::GeographicErr & err) {
+    std::cerr << err.what() << std::endl;
+    return mgrs_point;
+  }
+
+  // get mgrs values from utm values
+  mgrs_point.x() = fmod(utm_point.x(), 1e5);
+  mgrs_point.y() = fmod(utm_point.y(), 1e5);
+  projected_grid_ = mgrs_code;
+
+  if (!prev_projected_grid.empty() && prev_projected_grid != projected_grid_) {
+    std::cerr << "Projected MGRS Grid changed from last projection. Projected point"
+                 "might be far away from previously projected point."
+              << std::endl
+              << "You may want to use different projector.";
+  }
+
+  return mgrs_point;
+}
+
+GPSPoint MGRSProjector::reverse(const BasicPoint3d & mgrs_point) const
+{
+  GPSPoint gps{0., 0., 0.};
+  // reverse function cannot be used if mgrs_code_ is not set
+  if (isMGRSCodeSet()) {
+    gps = reverse(mgrs_point, mgrs_code_);
+  } else if (!projected_grid_.empty()) {
+    gps = reverse(mgrs_point, projected_grid_);
+  } else {
+    std::cerr << "cannot run reverse operation if mgrs code is not set in projector." << std::endl
+              << "use setMGRSCode function or explicitly give mgrs code as an argument.";
+  }
+  return gps;
+}
+
+GPSPoint MGRSProjector::reverse(const BasicPoint3d & mgrs_point, const std::string & mgrs_code)
+{
+  GPSPoint gps{0., 0., mgrs_point.z()};
+  BasicPoint3d utm_point{0., 0., gps.ele};
+
+  int zone{};
+  int prec{};
+  bool is_north{};
+  try {
+    GeographicLib::MGRS::Reverse(
+      mgrs_code, zone, is_north, utm_point.x(), utm_point.y(), prec, false);
+    utm_point.x() += fmod(mgrs_point.x(), pow(10, 5 - prec));
+    utm_point.y() += fmod(mgrs_point.y(), pow(10, 5 - prec));
+    GeographicLib::UTMUPS::Reverse(zone, is_north, utm_point.x(), utm_point.y(), gps.lat, gps.lon);
+  } catch (const GeographicLib::GeographicErr & err) {
+    std::cerr << "Failed to convert from MGRS to WGS";
+    return gps;
+  }
+
+  return gps;
+}
+
+void MGRSProjector::setMGRSCode(const std::string & mgrs_code)
+{
+  mgrs_code_ = mgrs_code;
+}
+
+void MGRSProjector::setMGRSCode(const GPSPoint & gps, const int precision)
+{
+  BasicPoint3d utm_point{0., 0., gps.ele};
+  int zone{};
+  bool is_north{};
+  std::string mgrs_code;
+
+  try {
+    GeographicLib::UTMUPS::Forward(gps.lat, gps.lon, zone, is_north, utm_point.x(), utm_point.y());
+    GeographicLib::MGRS::Forward(
+      zone, is_north, utm_point.x(), utm_point.y(), gps.lat, precision, mgrs_code);
+  } catch (const GeographicLib::GeographicErr & err) {
+    std::cerr << err.what() << std::endl;
+  }
+
+  setMGRSCode(mgrs_code);
+}
+
+}  // namespace lanelet::projection
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/no_parking_area.cpp b/lanelet2_extension/lib/no_parking_area.cpp
new file mode 100644
index 0000000..ee282ba
--- /dev/null
+++ b/lanelet2_extension/lib/no_parking_area.cpp
@@ -0,0 +1,134 @@
+// Copyright 2023 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T & primitive, RuleParameters * member)
+{
+  if (member == nullptr) {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end()) {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+template <typename T>
+Optional<T> tryGetFront(const std::vector<T> & vec)
+{
+  if (vec.empty()) {
+    return {};
+  }
+  return vec.front();
+}
+
+template <typename T>
+RuleParameters toRuleParameters(const std::vector<T> & primitives)
+{
+  auto cast_func = [](const auto & elem) { return static_cast<RuleParameter>(elem); };
+  return utils::transform(primitives, cast_func);
+}
+
+Polygons3d getPoly(const RuleParameterMap & paramsMap, RoleName role)
+{
+  auto params = paramsMap.find(role);
+  if (params == paramsMap.end()) {
+    return {};
+  }
+
+  Polygons3d result;
+  for (auto & param : params->second) {
+    auto p = boost::get<Polygon3d>(&param);
+    if (p != nullptr) {
+      result.push_back(*p);
+    }
+  }
+  return result;
+}
+
+ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role)
+{
+  auto cast_func = [](auto & poly) { return static_cast<ConstPolygon3d>(poly); };
+  return utils::transform(getPoly(params, role), cast_func);
+}
+
+RegulatoryElementDataPtr constructNoParkingAreaData(
+  Id id, const AttributeMap & attributes, const Polygons3d & NoParkingAreas)
+{
+  RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(NoParkingAreas)}};
+
+  auto data = std::make_shared<RegulatoryElementData>(id, std::move(rpm), attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "no_parking_area";
+  return data;
+}
+}  // namespace
+
+NoParkingArea::NoParkingArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getConstPoly(data->parameters, RoleName::Refers).empty()) {
+    throw InvalidInputError("no parking area defined!");
+  }
+}
+
+NoParkingArea::NoParkingArea(
+  Id id, const AttributeMap & attributes, const Polygons3d & no_parking_areas)
+: NoParkingArea(constructNoParkingAreaData(id, attributes, no_parking_areas))
+{
+}
+
+ConstPolygons3d NoParkingArea::noParkingAreas() const
+{
+  return getConstPoly(parameters(), RoleName::Refers);
+}
+Polygons3d NoParkingArea::noParkingAreas()
+{
+  return getPoly(parameters(), RoleName::Refers);
+}
+
+void NoParkingArea::addNoParkingArea(const Polygon3d & primitive)
+{
+  parameters()["no_parking_area"].emplace_back(primitive);
+}
+
+bool NoParkingArea::removeNoParkingArea(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("no_parking_area")->second);
+}
+
+RegisterRegulatoryElement<NoParkingArea> regNoParkingArea;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/no_stopping_area.cpp b/lanelet2_extension/lib/no_stopping_area.cpp
new file mode 100644
index 0000000..afade90
--- /dev/null
+++ b/lanelet2_extension/lib/no_stopping_area.cpp
@@ -0,0 +1,162 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T & primitive, RuleParameters * member)
+{
+  if (member == nullptr) {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end()) {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+template <typename T>
+Optional<T> tryGetFront(const std::vector<T> & vec)
+{
+  if (vec.empty()) {
+    return {};
+  }
+  return vec.front();
+}
+
+template <typename T>
+RuleParameters toRuleParameters(const std::vector<T> & primitives)
+{
+  auto cast_func = [](const auto & elem) { return static_cast<RuleParameter>(elem); };
+  return utils::transform(primitives, cast_func);
+}
+
+Polygons3d getPoly(const RuleParameterMap & paramsMap, RoleName role)
+{
+  auto params = paramsMap.find(role);
+  if (params == paramsMap.end()) {
+    return {};
+  }
+
+  Polygons3d result;
+  for (auto & param : params->second) {
+    auto p = boost::get<Polygon3d>(&param);
+    if (p != nullptr) {
+      result.push_back(*p);
+    }
+  }
+  return result;
+}
+
+ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role)
+{
+  auto cast_func = [](auto & poly) { return static_cast<ConstPolygon3d>(poly); };
+  return utils::transform(getPoly(params, role), cast_func);
+}
+
+RegulatoryElementDataPtr constructNoStoppingAreaData(
+  Id id, const AttributeMap & attributes, const Polygons3d & noStoppingAreas,
+  const Optional<LineString3d> & stopLine = {})
+{
+  RuleParameterMap rpm = {{RoleNameString::Refers, toRuleParameters(noStoppingAreas)}};
+  if (!!stopLine) {
+    rpm.insert({RoleNameString::RefLine, {*stopLine}});
+  }
+
+  auto data = std::make_shared<RegulatoryElementData>(id, std::move(rpm), attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "no_stopping_area";
+  return data;
+}
+}  // namespace
+
+NoStoppingArea::NoStoppingArea(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getConstPoly(data->parameters, RoleName::Refers).empty()) {
+    throw InvalidInputError("no stopping area defined!");
+  }
+  if (getParameters<ConstLineString3d>(RoleName::RefLine).size() > 1) {
+    throw InvalidInputError("There can not exist more than one stop line!");
+  }
+}
+
+NoStoppingArea::NoStoppingArea(
+  Id id, const AttributeMap & attributes, const Polygons3d & no_stopping_areas,
+  const Optional<LineString3d> & stopLine)
+: NoStoppingArea(constructNoStoppingAreaData(id, attributes, no_stopping_areas, stopLine))
+{
+}
+
+ConstPolygons3d NoStoppingArea::noStoppingAreas() const
+{
+  return getConstPoly(parameters(), RoleName::Refers);
+}
+Polygons3d NoStoppingArea::noStoppingAreas()
+{
+  return getPoly(parameters(), RoleName::Refers);
+}
+
+void NoStoppingArea::addNoStoppingArea(const Polygon3d & primitive)
+{
+  parameters()["no_stopping_area"].emplace_back(primitive);
+}
+
+bool NoStoppingArea::removeNoStoppingArea(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("no_stopping_area")->second);
+}
+
+Optional<ConstLineString3d> NoStoppingArea::stopLine() const
+{
+  return tryGetFront(getParameters<ConstLineString3d>(RoleName::RefLine));
+}
+
+Optional<LineString3d> NoStoppingArea::stopLine()
+{
+  return tryGetFront(getParameters<LineString3d>(RoleName::RefLine));
+}
+
+void NoStoppingArea::setStopLine(const LineString3d & stopLine)
+{
+  parameters()[RoleName::RefLine] = {stopLine};
+}
+
+void NoStoppingArea::removeStopLine()
+{
+  parameters()[RoleName::RefLine] = {};
+}
+
+RegisterRegulatoryElement<NoStoppingArea> regNoStoppingArea;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/query.cpp b/lanelet2_extension/lib/query.cpp
new file mode 100644
index 0000000..a505c12
--- /dev/null
+++ b/lanelet2_extension/lib/query.cpp
@@ -0,0 +1,1125 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/utility/query.hpp"
+
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+#include "lanelet2_extension/regulatory_elements/crosswalk.hpp"
+#include "lanelet2_extension/regulatory_elements/detection_area.hpp"
+#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp"
+#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
+#include "lanelet2_extension/regulatory_elements/speed_bump.hpp"
+#include "lanelet2_extension/utility/message_conversion.hpp"
+#include "lanelet2_extension/utility/utilities.hpp"
+
+#include <Eigen/Eigen>
+#include <autoware_utils/autoware_utils.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <tf2/utils.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#else
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#endif
+
+#include <deque>
+#include <limits>
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+using lanelet::utils::to2D;
+
+namespace lanelet::utils
+{
+// returns all lanelets in laneletLayer - don't know how to convert
+// PrimitiveLayer<Lanelets> -> std::vector<Lanelets>
+lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map)
+{
+  lanelet::ConstLanelets lanelets;
+  if (!ll_map) {
+    std::cerr << "No map received!";
+    return lanelets;
+  }
+
+  for (const auto & li : ll_map->laneletLayer) {
+    lanelets.push_back(li);
+  }
+
+  return lanelets;
+}
+
+lanelet::ConstLanelets query::subtypeLanelets(
+  const lanelet::ConstLanelets & lls, const char subtype[])
+{
+  lanelet::ConstLanelets subtype_lanelets;
+
+  for (const auto & ll : lls) {
+    if (ll.hasAttribute(lanelet::AttributeName::Subtype)) {
+      const lanelet::Attribute & attr = ll.attribute(lanelet::AttributeName::Subtype);
+      if (attr.value() == subtype) {
+        subtype_lanelets.push_back(ll);
+      }
+    }
+  }
+
+  return subtype_lanelets;
+}
+
+lanelet::ConstLanelets query::crosswalkLanelets(const lanelet::ConstLanelets & lls)
+{
+  return query::subtypeLanelets(lls, lanelet::AttributeValueString::Crosswalk);
+}
+
+lanelet::ConstLanelets query::walkwayLanelets(const lanelet::ConstLanelets & lls)
+{
+  return query::subtypeLanelets(lls, lanelet::AttributeValueString::Walkway);
+}
+
+lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets & lls)
+{
+  return query::subtypeLanelets(lls, lanelet::AttributeValueString::Road);
+}
+
+lanelet::ConstLanelets query::shoulderLanelets(const lanelet::ConstLanelets & lls)
+{
+  return query::subtypeLanelets(lls, "road_shoulder");
+}
+
+std::vector<lanelet::TrafficLightConstPtr> query::trafficLights(
+  const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::TrafficLightConstPtr> ll_tl_re =
+      ll.regulatoryElementsAs<lanelet::TrafficLight>();
+
+    // insert unique tl into array
+    for (const auto & tl_ptr : ll_tl_re) {
+      lanelet::Id id = tl_ptr->id();
+      bool unique_id = true;
+      for (const auto & tl_reg_elem : tl_reg_elems) {
+        if (id == tl_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+      if (unique_id) {
+        tl_reg_elems.push_back(tl_ptr);
+      }
+    }
+  }
+  return tl_reg_elems;
+}
+
+std::vector<lanelet::AutowareTrafficLightConstPtr> query::autowareTrafficLights(
+  const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::AutowareTrafficLightConstPtr> tl_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::AutowareTrafficLightConstPtr> ll_tl_re =
+      ll.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>();
+
+    // insert unique tl into array
+    for (const auto & tl_ptr : ll_tl_re) {
+      lanelet::Id id = tl_ptr->id();
+      bool unique_id = true;
+
+      for (const auto & tl_reg_elem : tl_reg_elems) {
+        if (id == tl_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id) {
+        tl_reg_elems.push_back(tl_ptr);
+      }
+    }
+  }
+  return tl_reg_elems;
+}
+
+std::vector<lanelet::DetectionAreaConstPtr> query::detectionAreas(
+  const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::DetectionAreaConstPtr> da_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::DetectionAreaConstPtr> ll_da_re =
+      ll.regulatoryElementsAs<lanelet::autoware::DetectionArea>();
+
+    // insert unique tl into array
+    for (const auto & da_ptr : ll_da_re) {
+      lanelet::Id id = da_ptr->id();
+      bool unique_id = true;
+
+      for (const auto & da_reg_elem : da_reg_elems) {
+        if (id == da_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id) {
+        da_reg_elems.push_back(da_ptr);
+      }
+    }
+  }
+  return da_reg_elems;
+}
+
+std::vector<lanelet::NoStoppingAreaConstPtr> query::noStoppingAreas(
+  const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::NoStoppingAreaConstPtr> no_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::NoStoppingAreaConstPtr> ll_no_re =
+      ll.regulatoryElementsAs<lanelet::autoware::NoStoppingArea>();
+
+    // insert unique tl into array
+    for (const auto & no_ptr : ll_no_re) {
+      lanelet::Id id = no_ptr->id();
+      bool unique_id = true;
+
+      for (const auto & no_reg_elem : no_reg_elems) {
+        if (id == no_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id) {
+        no_reg_elems.push_back(no_ptr);
+      }
+    }
+  }
+  return no_reg_elems;
+}
+
+std::vector<lanelet::NoParkingAreaConstPtr> query::noParkingAreas(
+  const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::NoParkingAreaConstPtr> no_pa_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::NoParkingAreaConstPtr> ll_no_pa_re =
+      ll.regulatoryElementsAs<lanelet::autoware::NoParkingArea>();
+
+    // insert unique NoParkingArea into array
+    for (const auto & no_pa_ptr : ll_no_pa_re) {
+      lanelet::Id id = no_pa_ptr->id();
+      bool unique_id = true;
+
+      for (const auto & no_reg_elem : no_pa_reg_elems) {
+        if (id == no_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id) {
+        no_pa_reg_elems.push_back(no_pa_ptr);
+      }
+    }
+  }
+  return no_pa_reg_elems;
+}
+
+std::vector<lanelet::SpeedBumpConstPtr> query::speedBumps(const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::SpeedBumpConstPtr> sb_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::SpeedBumpConstPtr> ll_sb_re =
+      ll.regulatoryElementsAs<lanelet::autoware::SpeedBump>();
+
+    // insert unique id into array
+    for (const auto & sb_ptr : ll_sb_re) {
+      lanelet::Id id = sb_ptr->id();
+      bool unique_id = true;
+
+      for (const auto & sb_reg_elem : sb_reg_elems) {
+        if (id == sb_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id) {
+        sb_reg_elems.push_back(sb_ptr);
+      }
+    }
+  }
+  return sb_reg_elems;
+}
+
+std::vector<lanelet::CrosswalkConstPtr> query::crosswalks(const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::CrosswalkConstPtr> cw_reg_elems;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::CrosswalkConstPtr> ll_cw_re =
+      ll.regulatoryElementsAs<lanelet::autoware::Crosswalk>();
+
+    // insert unique id into array
+    for (const auto & cw_ptr : ll_cw_re) {
+      lanelet::Id id = cw_ptr->id();
+      bool unique_id = true;
+
+      for (const auto & cw_reg_elem : cw_reg_elems) {
+        if (id == cw_reg_elem->id()) {
+          unique_id = false;
+          break;
+        }
+      }
+
+      if (unique_id) {
+        cw_reg_elems.push_back(cw_ptr);
+      }
+    }
+  }
+  return cw_reg_elems;
+}
+
+lanelet::ConstLineStrings3d query::curbstones(const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d curbstones;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "curbstone") {
+      curbstones.push_back(ls);
+    }
+  }
+  return curbstones;
+}
+
+lanelet::ConstPolygons3d query::getAllPolygonsByType(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type)
+{
+  lanelet::ConstPolygons3d polygons;
+  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
+    const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == polygon_type) {
+      polygons.push_back(poly);
+    }
+  }
+  return polygons;
+}
+
+lanelet::ConstPolygons3d query::getAllObstaclePolygons(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstPolygons3d obstacle_polygons;
+  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
+    const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "obstacle") {
+      obstacle_polygons.push_back(poly);
+    }
+  }
+  return obstacle_polygons;
+}
+
+lanelet::ConstPolygons3d query::getAllParkingLots(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstPolygons3d parking_lots;
+  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
+    const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "parking_lot") {
+      parking_lots.push_back(poly);
+    }
+  }
+  return parking_lots;
+}
+
+lanelet::ConstLineStrings3d query::getAllPartitions(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d partitions;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "guard_rail" || type == "fence" || type == "wall") {
+      partitions.push_back(ls);
+    }
+  }
+  return partitions;
+}
+
+lanelet::ConstLineStrings3d query::getAllFences(const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d fences;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "fence") {
+      fences.push_back(ls);
+    }
+  }
+  return fences;
+}
+
+lanelet::ConstLineStrings3d query::getAllPedestrianPolygonMarkings(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d pedestrian_polygon_markings;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if ((type == "pedestrian_marking") && (ls.size() >= 3)) {
+      pedestrian_polygon_markings.push_back(ls);
+    }
+  }
+  return pedestrian_polygon_markings;
+}
+
+lanelet::ConstLineStrings3d query::getAllPedestrianLineMarkings(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d pedestrian_line_markings;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if ((type == "pedestrian_marking") && (ls.size() < 3)) {
+      pedestrian_line_markings.push_back(ls);
+    }
+  }
+  return pedestrian_line_markings;
+}
+
+lanelet::ConstLineStrings3d query::getAllParkingSpaces(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d parking_spaces;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "parking_space") {
+      parking_spaces.push_back(ls);
+    }
+  }
+  return parking_spaces;
+}
+
+bool query::getLinkedLanelet(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet)
+{
+  const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr);
+  const auto & all_road_lanelets = query::roadLanelets(all_lanelets);
+  const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr);
+  return query::getLinkedLanelet(
+    parking_space, all_road_lanelets, all_parking_lots, linked_lanelet);
+}
+
+bool query::getLinkedLanelet(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstLanelets & all_road_lanelets,
+  const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet)
+{
+  const auto & linked_lanelets =
+    getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots);
+  if (linked_lanelets.empty()) {
+    return false;
+  }
+
+  double min_distance = std::numeric_limits<double>::max();
+  for (const auto & lanelet : linked_lanelets) {
+    const double distance = boost::geometry::distance(
+      to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon());
+    if (distance < min_distance) {
+      *linked_lanelet = lanelet;
+      min_distance = distance;
+    }
+  }
+  return true;
+}
+
+lanelet::ConstLanelets query::getLinkedLanelets(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr);
+  const auto & all_road_lanelets = query::roadLanelets(all_lanelets);
+  const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr);
+
+  return query::getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots);
+}
+
+lanelet::ConstLanelets query::getLinkedLanelets(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstLanelets & all_road_lanelets,
+  const lanelet::ConstPolygons3d & all_parking_lots)
+{
+  lanelet::ConstLanelets linked_lanelets;
+
+  // get lanelets within same parking lot
+  lanelet::ConstPolygon3d linked_parking_lot;
+  if (!getLinkedParkingLot(parking_space, all_parking_lots, &linked_parking_lot)) {
+    return linked_lanelets;
+  }
+  const auto & candidate_lanelets = getLinkedLanelets(linked_parking_lot, all_road_lanelets);
+
+  // get lanelets that are close to parking space and facing to parking space
+  for (const auto & lanelet : candidate_lanelets) {
+    // check if parking space is close to lanelet
+    const double distance = boost::geometry::distance(
+      to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon());
+    constexpr double distance_thresh = 5.0;
+    if (distance > distance_thresh) {
+      continue;
+    }
+
+    // check if parking space is facing lanelet
+    const Eigen::Vector3d direction =
+      parking_space.back().basicPoint() - parking_space.front().basicPoint();
+    const Eigen::Vector3d new_pt = parking_space.front().basicPoint() - direction * distance_thresh;
+
+    const lanelet::Point3d check_line_p1(lanelet::InvalId, new_pt.x(), new_pt.y(), new_pt.z());
+    const lanelet::Point3d check_line_p2(lanelet::InvalId, parking_space.back().basicPoint());
+    const lanelet::LineString3d check_line(lanelet::InvalId, {check_line_p1, check_line_p2});
+
+    const double new_distance = boost::geometry::distance(
+      to2D(check_line).basicLineString(), lanelet.polygon2d().basicPolygon());
+    if (new_distance < std::numeric_limits<double>::epsilon()) {
+      linked_lanelets.push_back(lanelet);
+    }
+  }
+
+  return linked_lanelets;
+}
+
+// get overlapping lanelets
+lanelet::ConstLanelets query::getLinkedLanelets(
+  const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets)
+{
+  lanelet::ConstLanelets linked_lanelets;
+  for (const auto & lanelet : all_road_lanelets) {
+    const double distance = boost::geometry::distance(
+      lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon());
+    if (distance < std::numeric_limits<double>::epsilon()) {
+      linked_lanelets.push_back(lanelet);
+    }
+  }
+  return linked_lanelets;
+}
+
+lanelet::ConstLineStrings3d query::getLinkedParkingSpaces(
+  const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  const auto & all_parking_spaces = query::getAllParkingSpaces(lanelet_map_ptr);
+  const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr);
+  return getLinkedParkingSpaces(lanelet, all_parking_spaces, all_parking_lots);
+}
+
+lanelet::ConstLineStrings3d query::getLinkedParkingSpaces(
+  const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces,
+  const lanelet::ConstPolygons3d & all_parking_lots)
+{
+  lanelet::ConstLineStrings3d linked_parking_spaces;
+
+  // get parking spaces that are in same parking lot.
+  lanelet::ConstPolygon3d linked_parking_lot;
+  if (!getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) {
+    return linked_parking_spaces;
+  }
+  const auto & possible_parking_spaces =
+    getLinkedParkingSpaces(linked_parking_lot, all_parking_spaces);
+
+  // check for parking spaces that are within 5m and facing towards lanelet
+  for (const auto & parking_space : possible_parking_spaces) {
+    // check if parking space is close to lanelet
+    const double distance = boost::geometry::distance(
+      to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon());
+    constexpr double distance_thresh = 5.0;
+    if (distance > distance_thresh) {
+      continue;
+    }
+
+    // check if parking space is facing lanelet
+    const Eigen::Vector3d direction =
+      parking_space.back().basicPoint() - parking_space.front().basicPoint();
+    const Eigen::Vector3d new_pt = parking_space.front().basicPoint() - direction * distance_thresh;
+
+    const lanelet::Point3d check_line_p1(lanelet::InvalId, new_pt.x(), new_pt.y(), new_pt.z());
+    const lanelet::Point3d check_line_p2(lanelet::InvalId, parking_space.back().basicPoint());
+    const lanelet::LineString3d check_line(lanelet::InvalId, {check_line_p1, check_line_p2});
+
+    const double new_distance = boost::geometry::distance(
+      to2D(check_line).basicLineString(), lanelet.polygon2d().basicPolygon());
+    if (new_distance < std::numeric_limits<double>::epsilon()) {
+      linked_parking_spaces.push_back(parking_space);
+    }
+  }
+  return linked_parking_spaces;
+}
+
+// get overlapping parking lot
+bool query::getLinkedParkingLot(
+  const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots,
+  lanelet::ConstPolygon3d * linked_parking_lot)
+{
+  for (const auto & parking_lot : all_parking_lots) {
+    const double distance = boost::geometry::distance(
+      lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon());
+    if (distance < std::numeric_limits<double>::epsilon()) {
+      *linked_parking_lot = parking_lot;
+      return true;
+    }
+  }
+  return false;
+}
+
+// get overlapping parking lot
+bool query::getLinkedParkingLot(
+  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots,
+  lanelet::ConstPolygon3d * linked_parking_lot)
+{
+  for (const auto & parking_lot : all_parking_lots) {
+    const double distance =
+      boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon());
+    if (distance < std::numeric_limits<double>::epsilon()) {
+      *linked_parking_lot = parking_lot;
+      return true;
+    }
+  }
+  return false;
+}
+
+// get overlapping parking lot
+bool query::getLinkedParkingLot(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot)
+{
+  for (const auto & parking_lot : all_parking_lots) {
+    const double distance = boost::geometry::distance(
+      to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon());
+    if (distance < std::numeric_limits<double>::epsilon()) {
+      *linked_parking_lot = parking_lot;
+      return true;
+    }
+  }
+  return false;
+}
+
+lanelet::ConstLineStrings3d query::getLinkedParkingSpaces(
+  const lanelet::ConstPolygon3d & parking_lot,
+  const lanelet::ConstLineStrings3d & all_parking_spaces)
+{
+  lanelet::ConstLineStrings3d linked_parking_spaces;
+  for (const auto & parking_space : all_parking_spaces) {
+    const double distance = boost::geometry::distance(
+      to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon());
+    if (distance < std::numeric_limits<double>::epsilon()) {
+      linked_parking_spaces.push_back(parking_space);
+    }
+  }
+  return linked_parking_spaces;
+}
+
+// return all stop lines and ref lines from a given set of lanelets
+std::vector<lanelet::ConstLineString3d> query::stopLinesLanelets(
+  const lanelet::ConstLanelets & lanelets)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+
+  for (const auto & ll : lanelets) {
+    std::vector<lanelet::ConstLineString3d> ll_stoplines;
+    ll_stoplines = query::stopLinesLanelet(ll);
+    stoplines.insert(stoplines.end(), ll_stoplines.begin(), ll_stoplines.end());
+  }
+
+  return stoplines;
+}
+
+// return all stop and ref lines from a given lanelet
+std::vector<lanelet::ConstLineString3d> query::stopLinesLanelet(const lanelet::ConstLanelet & ll)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+
+  // find stop lines referenced by right of way reg. elems.
+  std::vector<std::shared_ptr<const lanelet::RightOfWay>> right_of_way_reg_elems =
+    ll.regulatoryElementsAs<const lanelet::RightOfWay>();
+
+  if (!right_of_way_reg_elems.empty()) {
+    // lanelet has a right of way elem element
+    for (auto j = right_of_way_reg_elems.begin(); j < right_of_way_reg_elems.end(); j++) {
+      if ((*j)->getManeuver(ll) == lanelet::ManeuverType::Yield) {
+        // lanelet has a yield reg. elem.
+        lanelet::Optional<lanelet::ConstLineString3d> row_stopline_opt = (*j)->stopLine();
+        if (!!row_stopline_opt) {
+          stoplines.push_back(row_stopline_opt.get());
+        }
+      }
+    }
+  }
+
+  // find stop lines referenced by traffic lights
+  std::vector<std::shared_ptr<const lanelet::TrafficLight>> traffic_light_reg_elems =
+    ll.regulatoryElementsAs<const lanelet::TrafficLight>();
+
+  if (!traffic_light_reg_elems.empty()) {
+    // lanelet has a traffic light elem element
+    for (auto j = traffic_light_reg_elems.begin(); j < traffic_light_reg_elems.end(); j++) {
+      lanelet::Optional<lanelet::ConstLineString3d> traffic_light_stopline_opt = (*j)->stopLine();
+      if (!!traffic_light_stopline_opt) {
+        stoplines.push_back(traffic_light_stopline_opt.get());
+      }
+    }
+  }
+  // find stop lines referenced by traffic signs
+  std::vector<std::shared_ptr<const lanelet::TrafficSign>> traffic_sign_reg_elems =
+    ll.regulatoryElementsAs<const lanelet::TrafficSign>();
+
+  if (!traffic_sign_reg_elems.empty()) {
+    // lanelet has a traffic sign reg elem - can have multiple ref lines (but
+    // stop sign shod have 1
+    for (auto j = traffic_sign_reg_elems.begin(); j < traffic_sign_reg_elems.end(); j++) {
+      lanelet::ConstLineStrings3d traffic_sign_stoplines = (*j)->refLines();
+      if (!traffic_sign_stoplines.empty()) {
+        stoplines.push_back(traffic_sign_stoplines.front());
+      }
+    }
+  }
+  return stoplines;
+}
+
+std::vector<lanelet::ConstLineString3d> query::stopSignStopLines(
+  const lanelet::ConstLanelets & lanelets, const std::string & stop_sign_id)
+{
+  std::vector<lanelet::ConstLineString3d> stoplines;
+
+  std::set<lanelet::Id> checklist;
+
+  for (const auto & ll : lanelets) {
+    // find stop lines referenced by traffic signs
+    std::vector<std::shared_ptr<const lanelet::TrafficSign>> traffic_sign_reg_elems =
+      ll.regulatoryElementsAs<const lanelet::TrafficSign>();
+
+    if (!traffic_sign_reg_elems.empty()) {
+      // lanelet has a traffic sign reg elem - can have multiple ref lines (but
+      // stop sign shod have 1
+      for (const auto & ts : traffic_sign_reg_elems) {
+        // skip if traffic sign is not stop sign
+        if (ts->type() != stop_sign_id) {
+          continue;
+        }
+
+        lanelet::ConstLineStrings3d traffic_sign_stoplines = ts->refLines();
+
+        // only add new items
+        if (!traffic_sign_stoplines.empty()) {
+          auto id = traffic_sign_stoplines.front().id();
+          if (checklist.find(id) == checklist.end()) {
+            checklist.insert(id);
+            stoplines.push_back(traffic_sign_stoplines.front());
+          }
+        }
+      }
+    }
+  }
+  return stoplines;
+}
+
+ConstLanelets query::getLaneletsWithinRange(
+  const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point,
+  const double range)
+{
+  ConstLanelets near_lanelets;
+  for (const auto & ll : lanelets) {
+    lanelet::BasicPolygon2d poly = ll.polygon2d().basicPolygon();
+    double distance = lanelet::geometry::distance(poly, search_point);
+    if (distance <= range) {
+      near_lanelets.push_back(ll);
+    }
+  }
+  return near_lanelets;
+}
+
+ConstLanelets query::getLaneletsWithinRange(
+  const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point,
+  const double range)
+{
+  return getLaneletsWithinRange(
+    lanelets, lanelet::BasicPoint2d(search_point.x, search_point.y), range);
+}
+
+ConstLanelets query::getLaneChangeableNeighbors(
+  const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
+{
+  return graph->besides(lanelet);
+}
+
+ConstLanelets query::getLaneChangeableNeighbors(
+  const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets,
+  const geometry_msgs::msg::Point & search_point)
+{
+  const auto lanelets =
+    getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits<double>::epsilon());
+  ConstLanelets road_slices;
+  for (const auto & llt : lanelets) {
+    const auto tmp_road_slice = getLaneChangeableNeighbors(graph, llt);
+    road_slices.insert(road_slices.end(), tmp_road_slice.begin(), tmp_road_slice.end());
+  }
+  return road_slices;
+}
+
+ConstLanelets query::getAllNeighbors(
+  const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
+{
+  ConstLanelets lanelets;
+
+  ConstLanelets left_lanelets = getAllNeighborsLeft(graph, lanelet);
+  ConstLanelets right_lanelets = getAllNeighborsRight(graph, lanelet);
+
+  std::reverse(left_lanelets.begin(), left_lanelets.end());
+  lanelets.insert(lanelets.end(), left_lanelets.begin(), left_lanelets.end());
+  lanelets.push_back(lanelet);
+  lanelets.insert(lanelets.end(), right_lanelets.begin(), right_lanelets.end());
+
+  return lanelets;
+}
+
+ConstLanelets query::getAllNeighborsRight(
+  const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
+{
+  ConstLanelets lanelets;
+  auto right_lane =
+    (!!graph->right(lanelet)) ? graph->right(lanelet) : graph->adjacentRight(lanelet);
+  while (!!right_lane) {
+    lanelets.push_back(right_lane.get());
+    right_lane = (!!graph->right(right_lane.get())) ? graph->right(right_lane.get())
+                                                    : graph->adjacentRight(right_lane.get());
+  }
+  return lanelets;
+}
+
+ConstLanelets query::getAllNeighborsLeft(
+  const routing::RoutingGraphPtr & graph, const ConstLanelet & lanelet)
+{
+  ConstLanelets lanelets;
+  auto left_lane = (!!graph->left(lanelet)) ? graph->left(lanelet) : graph->adjacentLeft(lanelet);
+  while (!!left_lane) {
+    lanelets.push_back(left_lane.get());
+    left_lane = (!!graph->left(left_lane.get())) ? graph->left(left_lane.get())
+                                                 : graph->adjacentLeft(left_lane.get());
+  }
+  return lanelets;
+}
+
+ConstLanelets query::getAllNeighbors(
+  const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets,
+  const geometry_msgs::msg::Point & search_point)
+{
+  const auto lanelets =
+    getLaneletsWithinRange(road_lanelets, search_point, std::numeric_limits<double>::epsilon());
+  ConstLanelets road_slices;
+  for (const auto & llt : lanelets) {
+    const auto tmp_road_slice = getAllNeighbors(graph, llt);
+    road_slices.insert(road_slices.end(), tmp_road_slice.begin(), tmp_road_slice.end());
+  }
+  return road_slices;
+}
+
+bool query::getClosestLanelet(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelet * closest_lanelet_ptr)
+{
+  if (closest_lanelet_ptr == nullptr) {
+    std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet"
+              << std::endl;
+    return false;
+  }
+
+  if (lanelets.empty()) {
+    return false;
+  }
+
+  bool found = false;
+
+  lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y);
+
+  // find by distance
+  lanelet::ConstLanelets candidate_lanelets;
+  {
+    double min_distance = std::numeric_limits<double>::max();
+    for (const auto & llt : lanelets) {
+      double distance =
+        boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point);
+
+      if (std::abs(distance - min_distance) <= std::numeric_limits<double>::epsilon()) {
+        candidate_lanelets.push_back(llt);
+      } else if (distance < min_distance) {
+        found = true;
+        candidate_lanelets.clear();
+        candidate_lanelets.push_back(llt);
+        min_distance = distance;
+      }
+    }
+  }
+
+  if (candidate_lanelets.size() == 1) {
+    *closest_lanelet_ptr = candidate_lanelets.at(0);
+    return found;
+  }
+
+  // find by angle
+  {
+    double min_angle = std::numeric_limits<double>::max();
+    double pose_yaw = tf2::getYaw(search_pose.orientation);
+    for (const auto & llt : candidate_lanelets) {
+      lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline());
+      double angle_diff = M_PI;
+      if (!segment.empty()) {
+        double segment_angle = std::atan2(
+          segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x());
+        angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw));
+      }
+      if (angle_diff < min_angle) {
+        min_angle = angle_diff;
+        *closest_lanelet_ptr = llt;
+      }
+    }
+  }
+
+  return found;
+}
+
+bool query::getClosestLaneletWithConstrains(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelet * closest_lanelet_ptr, const double dist_threshold, const double yaw_threshold)
+{
+  bool found = false;
+
+  if (closest_lanelet_ptr == nullptr) {
+    std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet"
+              << std::endl;
+    return found;
+  }
+
+  if (lanelets.empty()) {
+    return found;
+  }
+
+  lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y);
+
+  // find by distance
+  std::vector<std::pair<lanelet::ConstLanelet, double>> candidate_lanelets;
+  {
+    for (const auto & llt : lanelets) {
+      double distance = boost::geometry::distance(llt.polygon2d().basicPolygon(), search_point);
+
+      if (distance <= dist_threshold) {
+        candidate_lanelets.emplace_back(llt, distance);
+      }
+    }
+
+    if (!candidate_lanelets.empty()) {
+      // sort by distance
+      std::sort(
+        candidate_lanelets.begin(), candidate_lanelets.end(),
+        [](
+          const std::pair<lanelet::ConstLanelet, double> & x,
+          std::pair<lanelet::ConstLanelet, double> & y) { return x.second < y.second; });
+    } else {
+      return found;
+    }
+  }
+
+  // find closest lanelet within yaw_threshold
+  {
+    double min_angle = std::numeric_limits<double>::max();
+    double min_distance = std::numeric_limits<double>::max();
+    double pose_yaw = tf2::getYaw(search_pose.orientation);
+    for (const auto & llt_pair : candidate_lanelets) {
+      const auto & distance = llt_pair.second;
+
+      double lanelet_angle = getLaneletAngle(llt_pair.first, search_pose.position);
+      double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw));
+
+      if (angle_diff > std::abs(yaw_threshold)) continue;
+      if (min_distance < distance) break;
+
+      if (angle_diff < min_angle) {
+        min_angle = angle_diff;
+        min_distance = distance;
+        *closest_lanelet_ptr = llt_pair.first;
+        found = true;
+      }
+    }
+  }
+
+  return found;
+}
+
+bool query::getCurrentLanelets(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point,
+  ConstLanelets * current_lanelets_ptr)
+{
+  if (current_lanelets_ptr == nullptr) {
+    std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet"
+              << std::endl;
+    return false;
+  }
+
+  if (lanelets.empty()) {
+    return false;
+  }
+
+  lanelet::BasicPoint2d search_point_2d(search_point.x, search_point.y);
+  for (const auto & llt : lanelets) {
+    if (lanelet::geometry::inside(llt, search_point_2d)) {
+      current_lanelets_ptr->push_back(llt);
+    }
+  }
+
+  return !current_lanelets_ptr->empty();  // return found
+}
+
+bool query::getCurrentLanelets(
+  const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
+  ConstLanelets * current_lanelets_ptr)
+{
+  if (current_lanelets_ptr == nullptr) {
+    std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet"
+              << std::endl;
+    return false;
+  }
+
+  if (lanelets.empty()) {
+    return false;
+  }
+
+  lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y);
+  for (const auto & llt : lanelets) {
+    if (lanelet::geometry::inside(llt, search_point)) {
+      current_lanelets_ptr->push_back(llt);
+    }
+  }
+
+  return !current_lanelets_ptr->empty();  // return found
+}
+
+std::vector<std::deque<lanelet::ConstLanelet>> getSucceedingLaneletSequencesRecursive(
+  const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length)
+{
+  std::vector<std::deque<lanelet::ConstLanelet>> succeeding_lanelet_sequences;
+
+  const auto next_lanelets = graph->following(lanelet);
+  const double lanelet_length = utils::getLaneletLength3d(lanelet);
+
+  // end condition of the recursive function
+  if (next_lanelets.empty() || lanelet_length >= length) {
+    succeeding_lanelet_sequences.push_back({lanelet});
+    return succeeding_lanelet_sequences;
+  }
+
+  for (const auto & next_lanelet : next_lanelets) {
+    // get lanelet sequence after next_lanelet
+    auto tmp_lanelet_sequences =
+      getSucceedingLaneletSequencesRecursive(graph, next_lanelet, length - lanelet_length);
+    for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) {
+      tmp_lanelet_sequence.push_front(lanelet);
+      succeeding_lanelet_sequences.push_back(tmp_lanelet_sequence);
+    }
+  }
+  return succeeding_lanelet_sequences;
+}
+
+std::vector<std::deque<lanelet::ConstLanelet>> getPrecedingLaneletSequencesRecursive(
+  const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length, const lanelet::ConstLanelets & exclude_lanelets)
+{
+  std::vector<std::deque<lanelet::ConstLanelet>> preceding_lanelet_sequences;
+
+  const auto prev_lanelets = graph->previous(lanelet);
+  const double lanelet_length = utils::getLaneletLength3d(lanelet);
+
+  // end condition of the recursive function
+  if (prev_lanelets.empty() || lanelet_length >= length) {
+    preceding_lanelet_sequences.push_back({lanelet});
+    return preceding_lanelet_sequences;
+  }
+
+  for (const auto & prev_lanelet : prev_lanelets) {
+    if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
+      // if prev_lanelet is included in exclude_lanelets,
+      // remove prev_lanelet from preceding_lanelet_sequences
+      continue;
+    }
+
+    // get lanelet sequence after prev_lanelet
+    auto tmp_lanelet_sequences = getPrecedingLaneletSequencesRecursive(
+      graph, prev_lanelet, length - lanelet_length, exclude_lanelets);
+    for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) {
+      tmp_lanelet_sequence.push_back(lanelet);
+      preceding_lanelet_sequences.push_back(tmp_lanelet_sequence);
+    }
+  }
+
+  if (preceding_lanelet_sequences.empty()) {
+    preceding_lanelet_sequences.push_back({lanelet});
+  }
+  return preceding_lanelet_sequences;
+}
+
+std::vector<lanelet::ConstLanelets> query::getSucceedingLaneletSequences(
+  const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length)
+{
+  std::vector<ConstLanelets> lanelet_sequences_vec;
+  const auto next_lanelets = graph->following(lanelet);
+  for (const auto & next_lanelet : next_lanelets) {
+    const auto lanelet_sequences_deq =
+      getSucceedingLaneletSequencesRecursive(graph, next_lanelet, length);
+    for (const auto & lanelet_sequence : lanelet_sequences_deq) {
+      lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end());
+    }
+  }
+  return lanelet_sequences_vec;
+}
+
+std::vector<lanelet::ConstLanelets> query::getPrecedingLaneletSequences(
+  const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length, const lanelet::ConstLanelets & exclude_lanelets)
+{
+  std::vector<ConstLanelets> lanelet_sequences_vec;
+  const auto prev_lanelets = graph->previous(lanelet);
+  for (const auto & prev_lanelet : prev_lanelets) {
+    if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
+      // if prev_lanelet is included in exclude_lanelets,
+      // remove prev_lanelet from preceding_lanelet_sequences
+      continue;
+    }
+    // convert deque into vector
+    const auto lanelet_sequences_deq =
+      getPrecedingLaneletSequencesRecursive(graph, prev_lanelet, length, exclude_lanelets);
+    for (const auto & lanelet_sequence : lanelet_sequences_deq) {
+      lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end());
+    }
+  }
+  return lanelet_sequences_vec;
+}
+}  // namespace lanelet::utils
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/road_marking.cpp b/lanelet2_extension/lib/road_marking.cpp
new file mode 100644
index 0000000..d514f7a
--- /dev/null
+++ b/lanelet2_extension/lib/road_marking.cpp
@@ -0,0 +1,82 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/road_marking.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+RegulatoryElementDataPtr constructRoadMarkingData(
+  Id id, const AttributeMap & attributes, const LineString3d & road_marking)
+{
+  RuleParameterMap rpm;
+  RuleParameters rule_parameters = {road_marking};
+  rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters));
+
+  auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "road_marking";
+  return data;
+}
+}  // namespace
+
+RoadMarking::RoadMarking(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getParameters<ConstLineString3d>(RoleName::Refers).size() != 1) {
+    throw InvalidInputError("There must be exactly one road marking defined!");
+  }
+}
+
+RoadMarking::RoadMarking(Id id, const AttributeMap & attributes, const LineString3d & road_marking)
+: RoadMarking(constructRoadMarkingData(id, attributes, road_marking))
+{
+}
+
+ConstLineString3d RoadMarking::roadMarking() const
+{
+  return getParameters<ConstLineString3d>(RoleName::Refers).front();
+}
+
+LineString3d RoadMarking::roadMarking()
+{
+  return getParameters<LineString3d>(RoleName::Refers).front();
+}
+
+void RoadMarking::setRoadMarking(const LineString3d & road_marking)
+{
+  parameters()[RoleName::Refers] = {road_marking};
+}
+
+void RoadMarking::removeRoadMarking()
+{
+  parameters()[RoleName::Refers] = {};
+}
+
+RegisterRegulatoryElement<RoadMarking> regRoadMarking;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/route_checker.cpp b/lanelet2_extension/lib/route_checker.cpp
new file mode 100644
index 0000000..796cfaf
--- /dev/null
+++ b/lanelet2_extension/lib/route_checker.cpp
@@ -0,0 +1,49 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/utility/route_checker.hpp"
+
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+
+namespace lanelet::utils
+{
+bool route::isRouteValid(
+  const LaneletRoute & route_msg, const lanelet::LaneletMapPtr lanelet_map_ptr_)
+{
+  for (const auto & route_section : route_msg.segments) {
+    for (const auto & primitive : route_section.primitives) {
+      const auto id = primitive.id;
+      try {
+        lanelet_map_ptr_->laneletLayer.get(id);
+      } catch (const std::exception & e) {
+        std::cerr
+          << e.what()
+          << ". Maybe the loaded route was created on a different Map from the current one. "
+             "Try to load the other Route again."
+          << std::endl;
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+}  // namespace lanelet::utils
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/speed_bump.cpp b/lanelet2_extension/lib/speed_bump.cpp
new file mode 100644
index 0000000..22e7c2b
--- /dev/null
+++ b/lanelet2_extension/lib/speed_bump.cpp
@@ -0,0 +1,122 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Ryohsuke Mitsudome, Mehmet Dogru
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/speed_bump.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+template <typename T>
+bool findAndErase(const T & primitive, RuleParameters * member)
+{
+  if (member == nullptr) {
+    std::cerr << __FUNCTION__ << ": member is null pointer";
+    return false;
+  }
+  auto it = std::find(member->begin(), member->end(), RuleParameter(primitive));
+  if (it == member->end()) {
+    return false;
+  }
+  member->erase(it);
+  return true;
+}
+
+Polygons3d getPoly(const RuleParameterMap & paramsMap, RoleName role)
+{
+  auto params = paramsMap.find(role);
+  if (params == paramsMap.end()) {
+    return {};
+  }
+
+  Polygons3d result;
+  for (auto & param : params->second) {
+    auto p = boost::get<Polygon3d>(&param);
+    if (p != nullptr) {
+      result.push_back(*p);
+    }
+  }
+  return result;
+}
+
+ConstPolygons3d getConstPoly(const RuleParameterMap & params, RoleName role)
+{
+  auto cast_func = [](auto & poly) { return static_cast<ConstPolygon3d>(poly); };
+  return utils::transform(getPoly(params, role), cast_func);
+}
+
+RegulatoryElementDataPtr constructSpeedBumpData(
+  Id id, const AttributeMap & attributes, const Polygon3d & speed_bump)
+{
+  RuleParameterMap rpm;
+  RuleParameters rule_parameters = {speed_bump};
+  rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters));
+
+  auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "speed_bump";
+  return data;
+}
+}  // namespace
+
+SpeedBump::SpeedBump(const RegulatoryElementDataPtr & data) : RegulatoryElement(data)
+{
+  if (getConstPoly(data->parameters, RoleName::Refers).size() != 1) {
+    throw InvalidInputError("There must be exactly one speed bump defined!");
+  }
+}
+
+SpeedBump::SpeedBump(Id id, const AttributeMap & attributes, const Polygon3d & speed_bump)
+: SpeedBump(constructSpeedBumpData(id, attributes, speed_bump))
+{
+}
+
+ConstPolygon3d SpeedBump::speedBump() const
+{
+  return getConstPoly(parameters(), RoleName::Refers).front();
+}
+
+Polygon3d SpeedBump::speedBump()
+{
+  return getPoly(parameters(), RoleName::Refers).front();
+}
+
+void SpeedBump::addSpeedBump(const Polygon3d & primitive)
+{
+  parameters()["speed_bump"].emplace_back(primitive);
+}
+
+bool SpeedBump::removeSpeedBump(const Polygon3d & primitive)
+{
+  return findAndErase(primitive, &parameters().find("speed_bump")->second);
+}
+
+RegisterRegulatoryElement<SpeedBump> regSpeedBump;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/transverse_mercator_projector.cpp b/lanelet2_extension/lib/transverse_mercator_projector.cpp
new file mode 100644
index 0000000..1bc5f8b
--- /dev/null
+++ b/lanelet2_extension/lib/transverse_mercator_projector.cpp
@@ -0,0 +1,64 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming, cppcoreguidelines-pro-type-member-init)
+
+#include "lanelet2_extension/projection/transverse_mercator_projector.hpp"
+
+#include <iostream>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace lanelet::projection
+{
+TransverseMercatorProjector::TransverseMercatorProjector(Origin origin) : Projector(origin)
+{
+  central_meridian_ = origin.position.lon;
+
+  // Calculate origin in Transverse Mercator coordinate
+  const GeographicLib::TransverseMercatorExact & proj =
+    GeographicLib::TransverseMercatorExact::UTM();
+  proj.Forward(central_meridian_, origin.position.lat, origin.position.lon, origin_x_, origin_y_);
+}
+
+BasicPoint3d TransverseMercatorProjector::forward(const GPSPoint & gps) const
+{
+  BasicPoint3d tm_point{0., 0., gps.ele};
+  const GeographicLib::TransverseMercatorExact & proj =
+    GeographicLib::TransverseMercatorExact::UTM();
+  proj.Forward(central_meridian_, gps.lat, gps.lon, tm_point.x(), tm_point.y());
+
+  // x is already aligned with origin as it is projected in transverse mercator
+  tm_point.y() = tm_point.y() - origin_y_;
+
+  return tm_point;
+}
+
+GPSPoint TransverseMercatorProjector::reverse(const BasicPoint3d & local_point) const
+{
+  GPSPoint gps{0.0, 0.0, local_point.z()};
+  const GeographicLib::TransverseMercatorExact & proj =
+    GeographicLib::TransverseMercatorExact::UTM();
+
+  BasicPoint3d local_point_copy = local_point;
+  local_point_copy.y() = local_point.y() + origin_y_;
+  proj.Reverse(central_meridian_, local_point_copy.x(), local_point_copy.y(), gps.lat, gps.lon);
+  return gps;
+}
+
+}  // namespace lanelet::projection
+
+// NOLINTEND(readability-identifier-naming, cppcoreguidelines-pro-type-member-init)
diff --git a/lanelet2_extension/lib/utilities.cpp b/lanelet2_extension/lib/utilities.cpp
new file mode 100644
index 0000000..8dbf514
--- /dev/null
+++ b/lanelet2_extension/lib/utilities.cpp
@@ -0,0 +1,795 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Kenji Miyake, Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/utility/utilities.hpp"
+
+#include "lanelet2_extension/utility/message_conversion.hpp"
+#include "lanelet2_extension/utility/query.hpp"
+
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Point.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_routing/Route.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRules.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <algorithm>
+#include <limits>
+#include <map>
+#include <utility>
+#include <vector>
+
+namespace lanelet::utils
+{
+namespace
+{
+
+/// @brief copy the z values between 2 containers based on the 2D arc lengths
+/// @tparam T1 a container of 3D points
+/// @tparam T2 a container of 3D points
+/// @param from points from which the z values will be copied
+/// @param to points to which the z values will be copied
+template <typename T1, typename T2>
+void copyZ(const T1 & from, T2 & to)
+{
+  if (from.empty() || to.empty()) return;
+  to.front().z() = from.front().z();
+  if (from.size() < 2 || to.size() < 2) return;
+  to.back().z() = from.back().z();
+  auto i_from = 1lu;
+  auto s_from = lanelet::geometry::distance2d(from[0], from[1]);
+  auto s_to = 0.0;
+  auto s_from_prev = 0.0;
+  for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) {
+    s_to += lanelet::geometry::distance2d(to[i_to - 1], to[i_to]);
+    for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) {
+      s_from_prev = s_from;
+      s_from += lanelet::geometry::distance2d(from[i_from], from[i_from + 1]);
+    }
+    const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev);
+    to[i_to].z() = from[i_from - 1].z() + ratio * (from[i_from].z() - from[i_from - 1].z());
+  }
+}
+
+[[maybe_unused]] bool exists(const std::vector<int> & array, const int element)
+{
+  return std::find(array.begin(), array.end(), element) != array.end();
+}
+
+/**
+ * [getContactingLanelets retrieves id of lanelets which has distance 0m to
+ * search_point]
+ * @param  lanelet_map   [pointer to lanelet]
+ * @param  trafficRules  [traffic rules to ignore lanelets that are not
+ * traversable]
+ * @param  search_point  [2D point used for searching]
+ * @param  contacting_lanelet_ids [array of lanelet ids that is contacting with
+ * search_point]
+ */
+[[maybe_unused]] void getContactingLanelets(
+  const lanelet::LaneletMapPtr lanelet_map,
+  const lanelet::traffic_rules::TrafficRulesPtr traffic_rules,
+  const lanelet::BasicPoint2d & search_point, std::vector<int> * contacting_lanelet_ids)
+{
+  if (!lanelet_map) {
+    std::cerr << "No lanelet map is set!" << std::endl;
+    return;
+  }
+
+  if (contacting_lanelet_ids == nullptr) {
+    std::cerr << __FUNCTION__ << " contacting_lanelet_ids is null pointer!" << std::endl;
+    return;
+  }
+
+  for (const auto & ll : lanelet_map->laneletLayer) {
+    if (!traffic_rules->canPass(ll)) {
+      continue;
+    }
+    lanelet::BasicPolygon2d poly = ll.polygon2d().basicPolygon();
+    double distance = lanelet::geometry::distance(poly, search_point);
+    if (distance < std::numeric_limits<double>::epsilon()) {
+      contacting_lanelet_ids->push_back(static_cast<int32_t>(ll.id()));
+    }
+  }
+}
+
+std::vector<double> calculateSegmentDistances(const lanelet::ConstLineString3d & line_string)
+{
+  std::vector<double> segment_distances;
+  segment_distances.reserve(line_string.size() - 1);
+
+  for (size_t i = 1; i < line_string.size(); ++i) {
+    const auto distance = lanelet::geometry::distance(line_string[i], line_string[i - 1]);
+    segment_distances.push_back(distance);
+  }
+
+  return segment_distances;
+}
+
+std::vector<double> calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string)
+{
+  const auto segment_distances = calculateSegmentDistances(line_string);
+
+  std::vector<double> accumulated_lengths{0};
+  accumulated_lengths.reserve(segment_distances.size() + 1);
+  std::partial_sum(
+    std::begin(segment_distances), std::end(segment_distances),
+    std::back_inserter(accumulated_lengths));
+
+  return accumulated_lengths;
+}
+
+std::pair<size_t, size_t> findNearestIndexPair(
+  const std::vector<double> & accumulated_lengths, const double target_length)
+{
+  // List size
+  const auto N = accumulated_lengths.size();
+
+  // Front
+  if (target_length < accumulated_lengths.at(1)) {
+    return std::make_pair(0, 1);
+  }
+
+  // Back
+  if (target_length > accumulated_lengths.at(N - 2)) {
+    return std::make_pair(N - 2, N - 1);
+  }
+
+  // Middle
+  for (std::size_t i = 1; i < N; ++i) {
+    if (
+      accumulated_lengths.at(i - 1) <= target_length &&
+      target_length <= accumulated_lengths.at(i)) {
+      return std::make_pair(i - 1, i);
+    }
+  }
+
+  // Throw an exception because this never happens
+  throw std::runtime_error("No nearest point found.");
+}
+
+std::vector<lanelet::BasicPoint3d> resamplePoints(
+  const lanelet::ConstLineString3d & line_string, const int num_segments)
+{
+  // Calculate length
+  const auto line_length = static_cast<double>(lanelet::geometry::length(line_string));
+
+  // Calculate accumulated lengths
+  const auto accumulated_lengths = calculateAccumulatedLengths(line_string);
+  if (accumulated_lengths.size() < 2) return {};
+
+  // Create each segment
+  std::vector<lanelet::BasicPoint3d> resampled_points;
+  for (auto i = 0; i <= num_segments; ++i) {
+    // Find two nearest points
+    const auto target_length = (static_cast<double>(i) / num_segments) * line_length;
+    const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length);
+
+    // Apply linear interpolation
+    const lanelet::BasicPoint3d back_point = line_string[index_pair.first];
+    const lanelet::BasicPoint3d front_point = line_string[index_pair.second];
+    const auto direction_vector = (front_point - back_point);
+
+    const auto back_length = accumulated_lengths.at(index_pair.first);
+    const auto front_length = accumulated_lengths.at(index_pair.second);
+    const auto segment_length = front_length - back_length;
+    const auto target_point =
+      back_point + (direction_vector * (target_length - back_length) / segment_length);
+
+    // Add to list
+    resampled_points.emplace_back(target_point);
+  }
+
+  return resampled_points;
+}
+lanelet::LineString3d getLineStringFromArcLength(
+  const lanelet::ConstLineString3d & linestring, const double s1, const double s2)
+{
+  lanelet::Points3d points;
+  double accumulated_length = 0;
+  size_t start_index = linestring.size();
+  if (start_index == 0) {
+    return lanelet::LineString3d{lanelet::InvalId, points};
+  }
+  for (size_t i = 0; i < linestring.size() - 1; i++) {
+    const auto & p1 = linestring[i];
+    const auto & p2 = linestring[i + 1];
+    const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint());
+    if (accumulated_length + length > s1) {
+      start_index = i;
+      break;
+    }
+    accumulated_length += length;
+  }
+  if (start_index < linestring.size() - 1) {
+    const auto & p1 = linestring[start_index];
+    const auto & p2 = linestring[start_index + 1];
+    const double residue = s1 - accumulated_length;
+    const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized();
+    const auto start_basic_point = p1.basicPoint() + residue * direction_vector;
+    const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point);
+    points.push_back(start_point);
+  }
+
+  accumulated_length = 0;
+  size_t end_index = linestring.size();
+  for (size_t i = 0; i < linestring.size() - 1; i++) {
+    const auto & p1 = linestring[i];
+    const auto & p2 = linestring[i + 1];
+    const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint());
+    if (accumulated_length + length > s2) {
+      end_index = i;
+      break;
+    }
+    accumulated_length += length;
+  }
+
+  for (size_t i = start_index + 1; i < end_index; i++) {
+    const auto p = lanelet::Point3d(linestring[i]);
+    points.push_back(p);
+  }
+  if (end_index < linestring.size() - 1) {
+    const auto & p1 = linestring[end_index];
+    const auto & p2 = linestring[end_index + 1];
+    const double residue = s2 - accumulated_length;
+    const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized();
+    const auto end_basic_point = p1.basicPoint() + residue * direction_vector;
+    const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point);
+    points.push_back(end_point);
+  }
+  return lanelet::LineString3d{lanelet::InvalId, points};
+}
+}  // namespace
+
+lanelet::ConstLanelet combineLaneletsShape(const lanelet::ConstLanelets & lanelets)
+{
+  lanelet::Points3d lefts;
+  lanelet::Points3d rights;
+  lanelet::Points3d centers;
+  for (const auto & llt : lanelets) {
+    for (const auto & pt : llt.leftBound()) {
+      lefts.push_back(lanelet::Point3d(pt));
+    }
+    for (const auto & pt : llt.rightBound()) {
+      rights.push_back(lanelet::Point3d(pt));
+    }
+    for (const auto & pt : llt.centerline()) {
+      centers.push_back(lanelet::Point3d(pt));
+    }
+  }
+  const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
+  const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
+  const auto center_line = lanelet::LineString3d(lanelet::InvalId, centers);
+  auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
+  combined_lanelet.setCenterline(center_line);
+  return std::move(combined_lanelet);
+}
+
+lanelet::LineString3d generateFineCenterline(
+  const lanelet::ConstLanelet & lanelet_obj, const double resolution)
+{
+  // Get length of longer border
+  const double left_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.leftBound()));
+  const double right_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.rightBound()));
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
+
+  // Resample points
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d centerline(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++) {
+    // Add ID for the average point of left and right
+    const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2;
+    const lanelet::Point3d center_point(
+      lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(),
+      center_basic_point.z());
+    centerline.push_back(center_point);
+  }
+  return centerline;
+}
+
+lanelet::ConstLineString3d getCenterlineWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution)
+{
+  // Get length of longer border
+  const double left_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.leftBound()));
+  const double right_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.rightBound()));
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
+
+  // Resample points
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d centerline(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++) {
+    // Add ID for the average point of left and right
+    const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2;
+
+    const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized();
+
+    const auto offset_center_basic_point = center_basic_point + vec_right_2_left * offset;
+
+    const lanelet::Point3d center_point(
+      lanelet::utils::getId(), offset_center_basic_point.x(), offset_center_basic_point.y(),
+      offset_center_basic_point.z());
+    centerline.push_back(center_point);
+  }
+  return static_cast<lanelet::ConstLineString3d>(centerline);
+}
+
+lanelet::ConstLineString3d getRightBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution)
+{
+  // Get length of longer border
+  const double left_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.leftBound()));
+  const double right_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.rightBound()));
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
+
+  // Resample points
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d rightBound(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++) {
+    // Add ID for the average point of left and right
+    const auto vec_left_2_right = (right_points.at(i) - left_points.at(i)).normalized();
+
+    const auto offset_right_basic_point = right_points.at(i) + vec_left_2_right * offset;
+
+    const lanelet::Point3d rightBound_point(
+      lanelet::utils::getId(), offset_right_basic_point.x(), offset_right_basic_point.y(),
+      offset_right_basic_point.z());
+    rightBound.push_back(rightBound_point);
+  }
+  return static_cast<lanelet::ConstLineString3d>(rightBound);
+}
+
+lanelet::ConstLineString3d getLeftBoundWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution)
+{
+  // Get length of longer border
+  const double left_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.leftBound()));
+  const double right_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.rightBound()));
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
+
+  // Resample points
+  const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d leftBound(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++) {
+    // Add ID for the average point of left and right
+
+    const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized();
+
+    const auto offset_left_basic_point = left_points.at(i) + vec_right_2_left * offset;
+
+    const lanelet::Point3d leftBound_point(
+      lanelet::utils::getId(), offset_left_basic_point.x(), offset_left_basic_point.y(),
+      offset_left_basic_point.z());
+    leftBound.push_back(leftBound_point);
+  }
+  return static_cast<lanelet::ConstLineString3d>(leftBound);
+}
+
+lanelet::ConstLanelet getExpandedLanelet(
+  const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset)
+{
+  using lanelet::geometry::offsetNoThrow;
+  using lanelet::geometry::internal::checkForInversion;
+
+  const auto & orig_left_bound_2d = lanelet_obj.leftBound2d().basicLineString();
+  const auto & orig_right_bound_2d = lanelet_obj.rightBound2d().basicLineString();
+
+  // Note: The lanelet::geometry::offset throws exception when the undesired inversion is found.
+  // Use offsetNoThrow until the logic is updated to handle the inversion.
+  // TODO(Horibe) update
+  auto expanded_left_bound_2d = offsetNoThrow(orig_left_bound_2d, left_offset);
+  auto expanded_right_bound_2d = offsetNoThrow(orig_right_bound_2d, right_offset);
+
+  rclcpp::Clock clock{RCL_ROS_TIME};
+  try {
+    checkForInversion(orig_left_bound_2d, expanded_left_bound_2d, left_offset);
+    checkForInversion(orig_right_bound_2d, expanded_right_bound_2d, right_offset);
+  } catch (const lanelet::GeometryError & e) {
+    RCLCPP_ERROR_THROTTLE(
+      rclcpp::get_logger("lanelet2_extension"), clock, 1000,
+      "Fail to expand lanelet. output may be undesired. Lanelet points interval in map data could "
+      "be too narrow.");
+  }
+
+  // Note: modify front and back points so that the successive lanelets will not have any
+  // longitudinal space between them.
+  {  // front
+    const double diff_x = orig_right_bound_2d.front().x() - orig_left_bound_2d.front().x();
+    const double diff_y = orig_right_bound_2d.front().y() - orig_left_bound_2d.front().y();
+    const double theta = std::atan2(diff_y, diff_x);
+    expanded_right_bound_2d.front().x() =
+      orig_right_bound_2d.front().x() - right_offset * std::cos(theta);
+    expanded_right_bound_2d.front().y() =
+      orig_right_bound_2d.front().y() - right_offset * std::sin(theta);
+    expanded_left_bound_2d.front().x() =
+      orig_left_bound_2d.front().x() - left_offset * std::cos(theta);
+    expanded_left_bound_2d.front().y() =
+      orig_left_bound_2d.front().y() - left_offset * std::sin(theta);
+  }
+  {  // back
+    const double diff_x = orig_right_bound_2d.back().x() - orig_left_bound_2d.back().x();
+    const double diff_y = orig_right_bound_2d.back().y() - orig_left_bound_2d.back().y();
+    const double theta = std::atan2(diff_y, diff_x);
+    expanded_right_bound_2d.back().x() =
+      orig_right_bound_2d.back().x() - right_offset * std::cos(theta);
+    expanded_right_bound_2d.back().y() =
+      orig_right_bound_2d.back().y() - right_offset * std::sin(theta);
+    expanded_left_bound_2d.back().x() =
+      orig_left_bound_2d.back().x() - left_offset * std::cos(theta);
+    expanded_left_bound_2d.back().y() =
+      orig_left_bound_2d.back().y() - left_offset * std::sin(theta);
+  }
+
+  const auto toPoints3d = [](const lanelet::BasicLineString2d & ls2d) {
+    lanelet::Points3d output;
+    for (const auto & pt : ls2d) {
+      output.push_back(lanelet::Point3d(lanelet::InvalId, pt.x(), pt.y(), 0.0));
+    }
+    return output;
+  };
+
+  lanelet::Points3d ex_lefts = toPoints3d(expanded_left_bound_2d);
+  lanelet::Points3d ex_rights = toPoints3d(expanded_right_bound_2d);
+  copyZ(lanelet_obj.leftBound3d(), ex_lefts);
+  copyZ(lanelet_obj.rightBound3d(), ex_rights);
+
+  const auto & extended_left_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_lefts);
+  const auto & expanded_right_bound_3d = lanelet::LineString3d(lanelet::InvalId, ex_rights);
+  const auto & lanelet = lanelet::Lanelet(
+    lanelet_obj.id(), extended_left_bound_3d, expanded_right_bound_3d, lanelet_obj.attributes());
+
+  return lanelet;
+}
+
+lanelet::ConstLanelets getExpandedLanelets(
+  const lanelet::ConstLanelets & lanelet_obj, const double left_offset, const double right_offset)
+{
+  lanelet::ConstLanelets lanelets;
+  for (const auto & llt : lanelet_obj) {
+    lanelets.push_back(getExpandedLanelet(llt, left_offset, right_offset));
+  }
+  return lanelets;
+}
+
+void overwriteLaneletsCenterline(
+  lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool force_overwrite)
+{
+  for (auto & lanelet_obj : lanelet_map->laneletLayer) {
+    if (force_overwrite || !lanelet_obj.hasCustomCenterline()) {
+      const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+      lanelet_obj.setCenterline(fine_center_line);
+    }
+  }
+}
+
+lanelet::ConstLanelets getConflictingLanelets(
+  const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet)
+{
+  const auto & llt_or_areas = graph->conflicting(lanelet);
+  lanelet::ConstLanelets lanelets;
+  lanelets.reserve(llt_or_areas.size());
+  for (const auto & l_or_a : llt_or_areas) {
+    auto llt_opt = l_or_a.lanelet();
+    if (!!llt_opt) {
+      lanelets.push_back(llt_opt.get());
+    }
+  }
+  return lanelets;
+}
+
+bool lineStringWithWidthToPolygon(
+  const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon)
+{
+  if (polygon == nullptr) {
+    std::cerr << __func__ << ": polygon is null pointer! Failed to convert to polygon."
+              << std::endl;
+    return false;
+  }
+  if (linestring.size() != 2) {
+    std::cerr << __func__ << ": linestring" << linestring.id() << " must have 2 points! ("
+              << linestring.size() << " != 2)" << std::endl
+              << "Failed to convert to polygon.";
+    return false;
+  }
+  if (!linestring.hasAttribute("width")) {
+    std::cerr << __func__ << ": linestring" << linestring.id()
+              << " does not have width tag. Failed to convert to polygon.";
+    return false;
+  }
+
+  const Eigen::Vector3d direction =
+    linestring.back().basicPoint() - linestring.front().basicPoint();
+  const double width = linestring.attributeOr("width", 0.0);
+  const Eigen::Vector3d direction_left =
+    (Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()) * direction).normalized();
+
+  const Eigen::Vector3d eigen_p1 = linestring.front().basicPoint() + direction_left * width / 2;
+  const Eigen::Vector3d eigen_p2 = linestring.back().basicPoint() + direction_left * width / 2;
+  const Eigen::Vector3d eigen_p3 = linestring.back().basicPoint() - direction_left * width / 2;
+  const Eigen::Vector3d eigen_p4 = linestring.front().basicPoint() - direction_left * width / 2;
+
+  const lanelet::Point3d p1(lanelet::InvalId, eigen_p1.x(), eigen_p1.y(), eigen_p1.z());
+  const lanelet::Point3d p2(lanelet::InvalId, eigen_p2.x(), eigen_p2.y(), eigen_p2.z());
+  const lanelet::Point3d p3(lanelet::InvalId, eigen_p3.x(), eigen_p3.y(), eigen_p3.z());
+  const lanelet::Point3d p4(lanelet::InvalId, eigen_p4.x(), eigen_p4.y(), eigen_p4.z());
+
+  *polygon = lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4});
+
+  return true;
+}
+
+bool lineStringToPolygon(
+  const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon)
+{
+  if (polygon == nullptr) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __func__ << ": polygon is null pointer! Failed to convert to polygon.");
+    return false;
+  }
+  if (linestring.size() < 4) {
+    if (linestring.size() < 3 || linestring.front().id() == linestring.back().id()) {
+      RCLCPP_WARN_STREAM(
+        rclcpp::get_logger("lanelet2_extension.visualization"),
+        __func__ << ": linestring" << linestring.id()
+                 << " must have more than different 3 points! (size is " << linestring.size()
+                 << "). Failed to convert to polygon.");
+      return false;
+    }
+  }
+
+  lanelet::Polygon3d llt_poly;
+
+  for (const auto & lp : linestring) {
+    llt_poly.push_back(lanelet::Point3d(
+      lanelet::InvalId, lp.basicPoint().x(), lp.basicPoint().y(), lp.basicPoint().z()));
+  }
+
+  if (linestring.front().id() == linestring.back().id()) {
+    llt_poly.pop_back();
+  }
+
+  *polygon = llt_poly;
+
+  return true;
+}
+
+double getLaneletLength2d(const lanelet::ConstLanelet & lanelet)
+{
+  return static_cast<double>(
+    boost::geometry::length(lanelet::utils::to2D(lanelet.centerline()).basicLineString()));
+}
+
+double getLaneletLength3d(const lanelet::ConstLanelet & lanelet)
+{
+  return static_cast<double>(boost::geometry::length(lanelet.centerline().basicLineString()));
+}
+
+double getLaneletLength2d(const lanelet::ConstLanelets & lanelet_sequence)
+{
+  double length = 0;
+  for (const auto & llt : lanelet_sequence) {
+    length += getLaneletLength2d(llt);
+  }
+  return length;
+}
+
+double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence)
+{
+  double length = 0;
+  for (const auto & llt : lanelet_sequence) {
+    length += getLaneletLength3d(llt);
+  }
+  return length;
+}
+
+lanelet::ArcCoordinates getArcCoordinates(
+  const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose)
+{
+  lanelet::ConstLanelet closest_lanelet;
+  lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet);
+
+  double length = 0;
+  lanelet::ArcCoordinates arc_coordinates;
+  for (const auto & llt : lanelet_sequence) {
+    const auto & centerline_2d = lanelet::utils::to2D(llt.centerline());
+    if (llt == closest_lanelet) {
+      const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position);
+      arc_coordinates = lanelet::geometry::toArcCoordinates(
+        centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
+      arc_coordinates.length += length;
+      break;
+    }
+    length += static_cast<double>(boost::geometry::length(centerline_2d));
+  }
+  return arc_coordinates;
+}
+
+lanelet::ConstLineString3d getClosestSegment(
+  const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring)
+{
+  if (linestring.size() < 2) {
+    return lanelet::LineString3d();
+  }
+
+  lanelet::ConstLineString3d closest_segment;
+  double min_distance = std::numeric_limits<double>::max();
+
+  for (size_t i = 1; i < linestring.size(); i++) {
+    lanelet::BasicPoint3d prev_basic_pt = linestring[i - 1].basicPoint();
+    lanelet::BasicPoint3d current_basic_pt = linestring[i].basicPoint();
+
+    lanelet::Point3d prev_pt(
+      lanelet::InvalId, prev_basic_pt.x(), prev_basic_pt.y(), prev_basic_pt.z());
+    lanelet::Point3d current_pt(
+      lanelet::InvalId, current_basic_pt.x(), current_basic_pt.y(), current_basic_pt.z());
+
+    lanelet::LineString3d current_segment(lanelet::InvalId, {prev_pt, current_pt});
+    double distance = lanelet::geometry::distance2d(
+      lanelet::utils::to2D(current_segment).basicLineString(), search_pt);
+    if (distance < min_distance) {
+      closest_segment = current_segment;
+      min_distance = distance;
+    }
+  }
+  return closest_segment;
+}
+
+lanelet::CompoundPolygon3d getPolygonFromArcLength(
+  const lanelet::ConstLanelets & lanelets, const double s1, const double s2)
+{
+  const auto combined_lanelet = combineLaneletsShape(lanelets);
+  const auto total_length = getLaneletLength2d(combined_lanelet);
+
+  // make sure that s1, and s2 are between [0, lane_length]
+  const auto s1_saturated = std::max(0.0, std::min(s1, total_length));
+  const auto s2_saturated = std::max(0.0, std::min(s2, total_length));
+
+  const auto ratio_s1 = s1_saturated / total_length;
+  const auto ratio_s2 = s2_saturated / total_length;
+
+  const auto s1_left = static_cast<double>(
+    ratio_s1 * boost::geometry::length(combined_lanelet.leftBound().basicLineString()));
+  const auto s2_left = static_cast<double>(
+    ratio_s2 * boost::geometry::length(combined_lanelet.leftBound().basicLineString()));
+  const auto s1_right = static_cast<double>(
+    ratio_s1 * boost::geometry::length(combined_lanelet.rightBound().basicLineString()));
+  const auto s2_right = static_cast<double>(
+    ratio_s2 * boost::geometry::length(combined_lanelet.rightBound().basicLineString()));
+
+  const auto left_bound =
+    getLineStringFromArcLength(combined_lanelet.leftBound(), s1_left, s2_left);
+  const auto right_bound =
+    getLineStringFromArcLength(combined_lanelet.rightBound(), s1_right, s2_right);
+
+  const auto & lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
+  return lanelet.polygon3d();
+}
+
+double getLaneletAngle(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point)
+{
+  lanelet::BasicPoint2d llt_search_point(search_point.x, search_point.y);
+  lanelet::ConstLineString3d segment = getClosestSegment(llt_search_point, lanelet.centerline());
+  return std::atan2(
+    segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x());
+}
+
+bool isInLanelet(
+  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
+  const double radius)
+{
+  constexpr double eps = 1.0e-9;
+  const lanelet::BasicPoint2d p(current_pose.position.x, current_pose.position.y);
+  return boost::geometry::distance(p, lanelet.polygon2d().basicPolygon()) < radius + eps;
+}
+
+geometry_msgs::msg::Pose getClosestCenterPose(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point)
+{
+  lanelet::BasicPoint2d llt_search_point(search_point.x, search_point.y);
+
+  if (lanelet.centerline().size() == 1) {
+    geometry_msgs::msg::Pose closest_pose;
+    closest_pose.position.x = lanelet.centerline().front().x();
+    closest_pose.position.y = lanelet.centerline().front().y();
+    closest_pose.position.z = search_point.z;
+    closest_pose.orientation.x = 0.0;
+    closest_pose.orientation.y = 0.0;
+    closest_pose.orientation.z = 0.0;
+    closest_pose.orientation.w = 1.0;
+    return closest_pose;
+  }
+
+  lanelet::ConstLineString3d segment = getClosestSegment(llt_search_point, lanelet.centerline());
+  if (segment.empty()) {
+    return geometry_msgs::msg::Pose{};
+  }
+
+  const Eigen::Vector2d direction(
+    (segment.back().basicPoint2d() - segment.front().basicPoint2d()).normalized());
+  const Eigen::Vector2d xf(segment.front().basicPoint2d());
+  const Eigen::Vector2d x(search_point.x, search_point.y);
+  const Eigen::Vector2d p = xf + (x - xf).dot(direction) * direction;
+
+  geometry_msgs::msg::Pose closest_pose;
+  closest_pose.position.x = p.x();
+  closest_pose.position.y = p.y();
+  closest_pose.position.z = search_point.z;
+
+  const double lane_yaw = getLaneletAngle(lanelet, search_point);
+  tf2::Quaternion q;
+  q.setRPY(0, 0, lane_yaw);
+  closest_pose.orientation = tf2::toMsg(q);
+
+  return closest_pose;
+}
+
+double getLateralDistanceToCenterline(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & pose)
+{
+  const auto & centerline_2d = lanelet::utils::to2D(lanelet.centerline());
+  const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position);
+  return lanelet::geometry::signedDistance(
+    centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
+}
+
+double getLateralDistanceToClosestLanelet(
+  const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose)
+{
+  lanelet::ConstLanelet closest_lanelet;
+  lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet);
+  return getLateralDistanceToCenterline(closest_lanelet, pose);
+}
+}  // namespace lanelet::utils
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/virtual_traffic_light.cpp b/lanelet2_extension/lib/virtual_traffic_light.cpp
new file mode 100644
index 0000000..7bf2f21
--- /dev/null
+++ b/lanelet2_extension/lib/virtual_traffic_light.cpp
@@ -0,0 +1,67 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp"
+
+#include <boost/variant.hpp>
+
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace lanelet::autoware
+{
+namespace
+{
+RegulatoryElementDataPtr constructVirtualTrafficLightData(
+  const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light)
+{
+  RuleParameterMap rpm;
+  RuleParameters rule_parameters = {virtual_traffic_light};
+  rpm.insert(std::make_pair(RoleNameString::Refers, rule_parameters));
+
+  auto data = std::make_shared<RegulatoryElementData>(id, rpm, attributes);
+  data->attributes[AttributeName::Type] = AttributeValueString::RegulatoryElement;
+  data->attributes[AttributeName::Subtype] = "virtual_traffic_light";
+  return data;
+}
+}  // namespace
+
+VirtualTrafficLight::VirtualTrafficLight(const RegulatoryElementDataPtr & data)
+: RegulatoryElement(data)
+{
+  if (getParameters<ConstLineString3d>("start_line").size() != 1) {
+    throw InvalidInputError("There must be exactly one start_line defined!");
+  }
+  if (getParameters<ConstLineString3d>("end_line").empty()) {
+    throw InvalidInputError("No end_line defined!");
+  }
+}
+
+VirtualTrafficLight::VirtualTrafficLight(
+  const Id id, const AttributeMap & attributes, const LineString3d & virtual_traffic_light)
+: VirtualTrafficLight(constructVirtualTrafficLightData(id, attributes, virtual_traffic_light))
+{
+}
+
+RegisterRegulatoryElement<VirtualTrafficLight> regVirtualTrafficLight;
+
+}  // namespace lanelet::autoware
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/lib/visualization.cpp b/lanelet2_extension/lib/visualization.cpp
new file mode 100644
index 0000000..0c48008
--- /dev/null
+++ b/lanelet2_extension/lib/visualization.cpp
@@ -0,0 +1,1647 @@
+// Copyright 2015-2019 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Simon Thompson, Ryohsuke Mitsudome
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/visualization/visualization.hpp"
+
+#include "lanelet2_extension/regulatory_elements/crosswalk.hpp"
+#include "lanelet2_extension/regulatory_elements/detection_area.hpp"
+#include "lanelet2_extension/regulatory_elements/speed_bump.hpp"
+#include "lanelet2_extension/utility/message_conversion.hpp"
+#include "lanelet2_extension/utility/query.hpp"
+#include "lanelet2_extension/utility/utilities.hpp"
+
+#include <Eigen/Eigen>
+
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <algorithm>
+#include <string>
+#include <unordered_set>
+#include <vector>
+
+namespace
+{
+template <typename T>
+bool exists(const std::unordered_set<T> & set, const T & element)
+{
+  return std::find(set.begin(), set.end(), element) != set.end();
+}
+
+void adjacentPoints(
+  const int i, const int N, const geometry_msgs::msg::Polygon & poly,
+  geometry_msgs::msg::Point32 * p0, geometry_msgs::msg::Point32 * p1,
+  geometry_msgs::msg::Point32 * p2)
+{
+  if (p0 == nullptr || p1 == nullptr || p2 == nullptr) {
+    std::cerr << __FUNCTION__ << ": either p0, p1, or p2 is null pointer!" << std::endl;
+    return;
+  }
+
+  *p1 = poly.points[i];
+  if (i == 0) {
+    *p0 = poly.points[N - 1];
+  } else {
+    *p0 = poly.points[i - 1];
+  }
+
+  if (i < N - 1) {
+    *p2 = poly.points[i + 1];
+  } else {
+    *p2 = poly.points[0];
+  }
+}
+
+[[maybe_unused]] double hypot(
+  const geometry_msgs::msg::Point32 & p0, const geometry_msgs::msg::Point32 & p1)
+{
+  return sqrt(pow((p1.x - p0.x), 2.0) + pow((p1.y - p0.y), 2.0));
+}
+
+bool isAttributeValue(
+  const lanelet::ConstPoint3d & p, const std::string & attr_str, const std::string & value_str)
+{
+  const lanelet::Attribute & attr = p.attribute(attr_str);
+  return attr.value() == value_str;
+}
+
+bool isLaneletAttributeValue(
+  const lanelet::ConstLanelet & ll, const std::string & attr_str, const std::string & value_str)
+{
+  const lanelet::Attribute & attr = ll.attribute(attr_str);
+  return attr.value() == value_str;
+}
+
+void initLightMarker(visualization_msgs::msg::Marker * marker, const std::string & ns)
+{
+  if (marker == nullptr) {
+    std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl;
+    return;
+  }
+
+  float s = 0.3;
+
+  marker->header.frame_id = "map";
+  marker->header.stamp = rclcpp::Time();
+  marker->frame_locked = false;
+  marker->ns = ns;
+  marker->id = 0;
+  marker->lifetime = rclcpp::Duration(0, 0);
+  marker->type = visualization_msgs::msg::Marker::SPHERE;
+  marker->scale.x = s;
+  marker->scale.y = s;
+  marker->scale.z = s;
+}
+
+bool inputLightMarker(visualization_msgs::msg::Marker * marker, const lanelet::ConstPoint3d & p)
+{
+  if (marker == nullptr) {
+    std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl;
+    return false;
+  }
+
+  marker->id = static_cast<int32_t>(p.id());
+
+  geometry_msgs::msg::Point point;
+  marker->pose.position.x = p.x();
+  marker->pose.position.y = p.y();
+  marker->pose.position.z = p.z();
+
+  std_msgs::msg::ColorRGBA color;
+  marker->color.r = 0.0f;
+  marker->color.g = 0.0f;
+  marker->color.b = 0.0f;
+  marker->color.a = 0.3f;
+
+  if (isAttributeValue(p, "color", "red")) {
+    marker->color.r = 0.3f;
+    marker->color.g = 0.0f;
+    marker->color.b = 0.0f;
+  } else if (isAttributeValue(p, "color", "green")) {
+    marker->color.r = 0.0f;
+    marker->color.g = 0.3f;
+    marker->color.b = 0.0f;
+  } else if (isAttributeValue(p, "color", "yellow")) {
+    marker->color.r = 0.3f;
+    marker->color.g = 0.3f;
+    marker->color.b = 0.0f;
+  } else {
+    marker->color.r = 0.3f;
+    marker->color.g = 0.3f;
+    marker->color.b = 0.3f;
+  }
+  return true;
+}
+
+void initLaneletDirectionMarker(visualization_msgs::msg::Marker * marker, const std::string & ns)
+{
+  if (marker == nullptr) {
+    std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl;
+    return;
+  }
+
+  float s = 1.0;
+
+  marker->header.frame_id = "map";
+  marker->header.stamp = rclcpp::Time();
+  marker->frame_locked = false;
+  marker->ns = ns;
+  marker->id = 0;
+  marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker->lifetime = rclcpp::Duration(0, 0);
+
+  marker->pose.position.x = 0.0;  // p.x();
+  marker->pose.position.y = 0.0;  // p.y();
+  marker->pose.position.z = 0.0;  // p.z();
+  marker->pose.orientation.x = 0.0;
+  marker->pose.orientation.y = 0.0;
+  marker->pose.orientation.z = 0.0;
+  marker->pose.orientation.w = 1.0;
+  marker->scale.x = s;
+  marker->scale.y = s;
+  marker->scale.z = s;
+  marker->color.r = 1.0f;
+  marker->color.g = 1.0f;
+  marker->color.b = 1.0f;
+  marker->color.a = 0.999;
+}
+
+void pushLaneletDirectionMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLanelet & ll)
+{
+  if (marker == nullptr) {
+    std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl;
+    return;
+  }
+
+  lanelet::BasicPoint3d pt[3];
+  pt[0].x() = 0.0;
+  pt[0].y() = -0.3;
+  pt[0].z() = 0.0;
+  pt[1].x() = 0.0;
+  pt[1].y() = 0.3;
+  pt[1].z() = 0;
+  pt[2].x() = 1.0;
+  pt[2].y() = 0.0;
+  pt[2].z() = 0;
+
+  lanelet::BasicPoint3d pc;
+  lanelet::BasicPoint3d pc2;
+  lanelet::ConstLineString3d center_ls = ll.centerline();
+  lanelet::Attribute attr = ll.attribute("turn_direction");
+
+  std_msgs::msg::ColorRGBA c;
+  c.r = 0.5;
+  c.g = 0.5;
+  c.b = 0.5;
+  c.a = 0.5;
+
+  if (isLaneletAttributeValue(ll, "turn_direction", "right")) {
+    c.r = 0.5;
+    c.g = 0.5;
+    c.b = 0.6;
+  } else if (isLaneletAttributeValue(ll, "turn_direction", "left")) {
+    c.r = 0.5;
+    c.g = 0.6;
+    c.b = 0.6;
+  }
+
+  for (std::size_t ci = 0; ci < center_ls.size() - 1;) {
+    pc = center_ls[ci];
+    if (center_ls.size() > 1) {
+      pc2 = center_ls[ci + 1];
+    } else {
+      return;
+    }
+
+    double heading = atan2(pc2.y() - pc.y(), pc2.x() - pc.x());
+
+    lanelet::BasicPoint3d pt_tf[3];
+
+    Eigen::Vector3d axis(0, 0, 1);
+    Eigen::Transform<double, 3, Eigen::Affine> t(Eigen::AngleAxis<double>(heading, axis));
+
+    for (int i = 0; i < 3; i++) {
+      pt_tf[i] = t * pt[i] + pc;
+    }
+
+    geometry_msgs::msg::Point gp[3];
+
+    for (int i = 0; i < 3; i++) {
+      std_msgs::msg::ColorRGBA cn = c;
+
+      gp[i].x = pt_tf[i].x();
+      gp[i].y = pt_tf[i].y();
+      gp[i].z = pt_tf[i].z();
+      marker->points.push_back(gp[i]);
+      marker->colors.push_back(cn);
+    }
+    ci = ci + 1;
+  }
+}
+
+bool isClockWise(const geometry_msgs::msg::Polygon & polygon)
+{
+  const int N = static_cast<int>(polygon.points.size());
+  const double x_offset = polygon.points[0].x;
+  const double y_offset = polygon.points[0].y;
+  double sum = 0.0;
+  for (std::size_t i = 0; i < polygon.points.size(); ++i) {
+    sum += (polygon.points[i].x - x_offset) * (polygon.points[(i + 1) % N].y - y_offset) -
+           (polygon.points[i].y - y_offset) * (polygon.points[(i + 1) % N].x - x_offset);
+  }
+
+  return sum < 0.0;
+}
+
+// Is angle AOB less than 180?
+// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e
+bool isAcuteAngle(
+  const geometry_msgs::msg::Point32 & a, const geometry_msgs::msg::Point32 & o,
+  const geometry_msgs::msg::Point32 & b)
+{
+  return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x) >= 0;
+}
+
+// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e
+bool isWithinTriangle(
+  const geometry_msgs::msg::Point32 & a, const geometry_msgs::msg::Point32 & b,
+  const geometry_msgs::msg::Point32 & c, const geometry_msgs::msg::Point32 & p)
+{
+  double c1 = (b.x - a.x) * (p.y - b.y) - (b.y - a.y) * (p.x - b.x);
+  double c2 = (c.x - b.x) * (p.y - c.y) - (c.y - b.y) * (p.x - c.x);
+  double c3 = (a.x - c.x) * (p.y - a.y) - (a.y - c.y) * (p.x - a.x);
+
+  return (c1 > 0.0 && c2 > 0.0 && c3 > 0.0) || (c1 < 0.0 && c2 < 0.0 && c3 < 0.0);
+}
+
+visualization_msgs::msg::Marker createPolygonMarker(
+  const std::string & name_space, const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::Marker marker;
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = false;
+  marker.id = 0;
+  marker.ns = name_space;
+  marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker.lifetime = rclcpp::Duration(0, 0);
+  marker.pose.position.x = 0.0;
+  marker.pose.position.y = 0.0;
+  marker.pose.position.z = 0.0;
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color = color;
+  return marker;
+}
+
+void pushPolygonMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstPolygon3d & polygon,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  if (marker == nullptr) {
+    std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl;
+    return;
+  }
+
+  if (polygon.size() < 3) {
+    return;
+  }
+
+  geometry_msgs::msg::Polygon geom_poly;
+  lanelet::utils::conversion::toGeomMsgPoly(polygon, &geom_poly);
+
+  std::vector<geometry_msgs::msg::Polygon> triangles;
+  lanelet::visualization::polygon2Triangle(geom_poly, &triangles);
+
+  for (const auto & tri : triangles) {
+    geometry_msgs::msg::Point geom_pts[3];
+    for (int i = 0; i < 3; i++) {
+      lanelet::utils::conversion::toGeomMsgPt(tri.points[i], &geom_pts[i]);
+      marker->points.push_back(geom_pts[i]);
+      marker->colors.push_back(color);
+    }
+  }
+}
+
+}  // anonymous namespace
+
+namespace lanelet
+{
+void visualization::lanelet2Triangle(
+  const lanelet::ConstLanelet & ll, std::vector<geometry_msgs::msg::Polygon> * triangles)
+{
+  if (triangles == nullptr) {
+    std::cerr << __FUNCTION__ << ": triangles is null pointer!" << std::endl;
+    return;
+  }
+
+  triangles->clear();
+  geometry_msgs::msg::Polygon ll_poly;
+  lanelet2Polygon(ll, &ll_poly);
+  polygon2Triangle(ll_poly, triangles);
+}
+
+// NOLINTBEGIN(readability-function-cognitive-complexity)
+void visualization::polygon2Triangle(
+  const geometry_msgs::msg::Polygon & polygon, std::vector<geometry_msgs::msg::Polygon> * triangles)
+{
+  geometry_msgs::msg::Polygon poly = polygon;
+  if (!isClockWise(poly)) {
+    std::reverse(poly.points.begin(), poly.points.end());
+  }
+
+  // ear clipping: find smallest internal angle in polygon
+  int N = static_cast<int>(poly.points.size());
+
+  // array of angles for each vertex
+  std::vector<bool> is_acute_angle;
+  is_acute_angle.assign(N, false);
+  for (int i = 0; i < N; i++) {
+    geometry_msgs::msg::Point32 p0;
+    geometry_msgs::msg::Point32 p1;
+    geometry_msgs::msg::Point32 p2;
+
+    adjacentPoints(i, N, poly, &p0, &p1, &p2);
+    is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2);
+  }
+
+  // start ear clipping
+  while (N >= 3) {
+    int clipped_vertex = -1;
+
+    for (int i = 0; i < N; i++) {
+      const bool theta = is_acute_angle.at(i);
+      if (theta) {
+        geometry_msgs::msg::Point32 p0;
+        geometry_msgs::msg::Point32 p1;
+        geometry_msgs::msg::Point32 p2;
+        adjacentPoints(i, N, poly, &p0, &p1, &p2);
+
+        int j_begin = (i + 2) % N;
+        int j_end = (i - 1 + N) % N;
+        bool is_ear = true;
+        for (int j = j_begin; j != j_end; j = (j + 1) % N) {
+          if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) {
+            is_ear = false;
+            break;
+          }
+        }
+
+        if (is_ear) {
+          clipped_vertex = i;
+          break;
+        }
+      }
+    }
+    if (clipped_vertex < 0 || clipped_vertex >= N) {
+      // print in yellow to indicate warning
+      std::cerr << "\033[1;33mCould not find valid vertex for ear clipping triangulation. "
+                   "Triangulation result might be invalid\033[0m"
+                << std::endl;
+      clipped_vertex = 0;
+    }
+
+    // create triangle
+    geometry_msgs::msg::Point32 p0;
+    geometry_msgs::msg::Point32 p1;
+    geometry_msgs::msg::Point32 p2;
+    adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2);
+    geometry_msgs::msg::Polygon triangle;
+    triangle.points.push_back(p0);
+    triangle.points.push_back(p1);
+    triangle.points.push_back(p2);
+    triangles->push_back(triangle);
+
+    // remove vertex of center of angle
+    auto it = poly.points.begin();
+    std::advance(it, clipped_vertex);
+    poly.points.erase(it);
+
+    // remove from angle list
+    auto it_angle = is_acute_angle.begin();
+    std::advance(it_angle, clipped_vertex);
+    is_acute_angle.erase(it_angle);
+
+    // update angle list
+    N = static_cast<int>(poly.points.size());
+    if (clipped_vertex == N) {
+      clipped_vertex = 0;
+    }
+    adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2);
+    is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2);
+
+    int i_prev = (clipped_vertex == 0) ? N - 1 : clipped_vertex - 1;
+    adjacentPoints(i_prev, N, poly, &p0, &p1, &p2);
+    is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2);
+  }
+}
+// NOLINTEND(readability-function-cognitive-complexity)
+
+void visualization::lanelet2Polygon(
+  const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon)
+{
+  if (polygon == nullptr) {
+    std::cerr << __FUNCTION__ << ": polygon is null pointer!" << std::endl;
+    return;
+  }
+
+  lanelet::CompoundPolygon3d ll_poly = ll.polygon3d();
+
+  polygon->points.clear();
+  polygon->points.reserve(ll_poly.size());
+
+  for (const auto & pt : ll_poly) {
+    geometry_msgs::msg::Point32 pt32;
+    utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32);
+    polygon->points.push_back(pt32);
+  }
+}
+
+visualization_msgs::msg::MarkerArray visualization::laneletDirectionAsMarkerArray(
+  const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  visualization_msgs::msg::Marker marker;
+  initLaneletDirectionMarker(&marker, additional_namespace + "lanelet direction");
+
+  for (const auto & ll : lanelets) {
+    if (ll.hasAttribute(std::string("turn_direction"))) {
+      pushLaneletDirectionMarker(&marker, ll);
+    }
+  }
+  if (marker.points.empty()) {
+    return marker_array;
+  }
+  marker_array.markers.push_back(marker);
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarkerArray(
+  const std::vector<lanelet::AutowareTrafficLightConstPtr> & tl_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale)
+{
+  visualization_msgs::msg::MarkerArray tl_marker_array;
+  if (tl_reg_elems.empty()) {
+    return tl_marker_array;
+  }
+  visualization_msgs::msg::Marker marker_tri;
+  visualization_msgs::msg::Marker marker_sph;
+  initLightMarker(&marker_sph, "traffic_light");
+  visualization::initTrafficLightTriangleMarker(&marker_tri, "traffic_light_triangle", duration);
+
+  for (const auto & tl : tl_reg_elems) {
+    const auto lights = tl->trafficLights();
+    for (const auto & lsp : lights) {
+      if (lsp.isLineString()) {  // traffic lights can either polygons or linestrings
+        lanelet::ConstLineString3d ls = static_cast<lanelet::ConstLineString3d>(lsp);
+        visualization::pushTrafficLightTriangleMarker(&marker_tri, ls, c, scale);
+      }
+    }
+    marker_tri.id++;
+    tl_marker_array.markers.push_back(marker_tri);
+
+    lanelet::ConstLineStrings3d light_bulbs = tl->lightBulbs();
+    for (const auto & ls : light_bulbs) {
+      lanelet::ConstLineString3d l = static_cast<lanelet::ConstLineString3d>(ls);
+      for (const auto & pt : l) {
+        if (pt.hasAttribute("color")) {
+          if (inputLightMarker(&marker_sph, pt)) {
+            marker_sph.id++;
+            tl_marker_array.markers.push_back(marker_sph);
+          }
+        }
+      }
+    }
+  }
+
+  return tl_marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::generateTrafficLightRegulatoryElementIdMaker(
+  const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration, const double scale)
+{
+  visualization_msgs::msg::MarkerArray tl_id_marker_array;
+
+  for (const auto & lanelet : lanelets) {
+    for (const auto & element : lanelet.regulatoryElementsAs<lanelet::TrafficLight>()) {
+      visualization_msgs::msg::Marker marker;
+      marker.header.frame_id = "map";
+      marker.header.stamp = rclcpp::Time();
+      marker.ns = "traffic_light_reg_elem_id";
+      marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+      marker.lifetime = duration;
+      marker.action = visualization_msgs::msg::Marker::ADD;
+      marker.pose.orientation.x = 0.0;
+      marker.pose.orientation.y = 0.0;
+      marker.pose.orientation.z = 0.0;
+      marker.pose.orientation.w = 1.0;
+      marker.color = c;
+      marker.scale.z = scale;
+      marker.frame_locked = false;
+
+      std::ostringstream string_stream;
+      string_stream << "TLRegElemId:" << std::to_string(element->id());
+      marker.text = string_stream.str();
+
+      marker.id = static_cast<int32_t>(lanelet.id());
+      marker.pose.position.x =
+        (lanelet.rightBound().front().x() + lanelet.leftBound().front().x()) / 2;
+      marker.pose.position.y =
+        (lanelet.rightBound().front().y() + lanelet.leftBound().front().y()) / 2;
+      marker.pose.position.z = lanelet.rightBound().front().z();
+      tl_id_marker_array.markers.push_back(marker);
+    }
+  }
+
+  return tl_id_marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker(
+  const std::vector<lanelet::AutowareTrafficLightConstPtr> & tl_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale)
+{
+  visualization_msgs::msg::MarkerArray tl_id_marker_array;
+
+  std::unordered_map<lanelet::Id, visualization_msgs::msg::Marker> traffic_light_map{};
+  for (const auto & element : tl_reg_elems) {
+    for (const auto & light : element->trafficLights()) {
+      if (!light.isLineString()) {
+        continue;
+      }
+      const auto line = static_cast<lanelet::ConstLineString3d>(light);
+      if (traffic_light_map.count(line.id()) == 0) {
+        visualization_msgs::msg::Marker marker;
+        marker.header.frame_id = "map";
+        marker.header.stamp = rclcpp::Time();
+        marker.ns = "traffic_light_id";
+        marker.id = static_cast<int32_t>(line.id());
+        marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+        marker.lifetime = duration;
+        marker.action = visualization_msgs::msg::Marker::ADD;
+        marker.pose.position.x = (line.front().x() + line.back().x()) / 2;
+        marker.pose.position.y = (line.front().y() + line.back().y()) / 2;
+        marker.pose.position.z = line.front().z() + 1.0;
+        marker.pose.orientation.x = 0.0;
+        marker.pose.orientation.y = 0.0;
+        marker.pose.orientation.z = 0.0;
+        marker.pose.orientation.w = 1.0;
+        marker.color = c;
+        marker.scale.z = scale;
+        marker.frame_locked = false;
+
+        std::ostringstream string_stream;
+        string_stream << "referrer:" << element->id() << ",";
+        marker.text = string_stream.str();
+        traffic_light_map.emplace(line.id(), marker);
+      } else {
+        std::ostringstream string_stream;
+        string_stream << element->id() << ",";
+        traffic_light_map.at(line.id()).text += string_stream.str();
+      }
+    }
+  }
+
+  for (const auto & [id, marker] : traffic_light_map) {
+    tl_id_marker_array.markers.push_back(marker);
+  }
+
+  return tl_id_marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray(
+  const std::vector<lanelet::DetectionAreaConstPtr> & da_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  visualization_msgs::msg::Marker marker;
+  visualization_msgs::msg::Marker line_marker;
+
+  if (da_reg_elems.empty()) {
+    return marker_array;
+  }
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = false;
+  marker.ns = "detection_area";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker.lifetime = duration;
+  marker.pose.position.x = 0.0;  // p.x();
+  marker.pose.position.y = 0.0;  // p.y();
+  marker.pose.position.z = 0.0;  // p.z();
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.r = 1.0f;
+  marker.color.g = 1.0f;
+  marker.color.b = 1.0f;
+  marker.color.a = 0.999;
+
+  std_msgs::msg::ColorRGBA line_c;
+  line_c.r = 0.5;
+  line_c.g = 0.5;
+  line_c.b = 0.5;
+  line_c.a = 0.999;
+  visualization::initLineStringMarker(&line_marker, "map", "detection_area_stopline", line_c);
+
+  for (const auto & da_reg_elem : da_reg_elems) {
+    marker.points.clear();
+    marker.colors.clear();
+    marker.id = static_cast<int32_t>(da_reg_elem->id());
+
+    // area visualization
+    const auto detection_areas = da_reg_elem->detectionAreas();
+    for (const auto & detection_area : detection_areas) {
+      geometry_msgs::msg::Polygon geom_poly;
+      utils::conversion::toGeomMsgPoly(detection_area, &geom_poly);
+
+      std::vector<geometry_msgs::msg::Polygon> triangles;
+      polygon2Triangle(geom_poly, &triangles);
+
+      for (auto tri : triangles) {
+        geometry_msgs::msg::Point tri0[3];
+
+        for (int i = 0; i < 3; i++) {
+          utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);
+          marker.points.push_back(tri0[i]);
+          marker.colors.push_back(c);
+        }
+      }  // for triangles0
+    }    // for detection areas
+    marker_array.markers.push_back(marker);
+
+    // stop line visualization
+    visualization::pushLineStringMarker(&line_marker, da_reg_elem->stopLine(), line_c, 0.5);
+  }  // for regulatory elements
+
+  marker_array.markers.push_back(line_marker);
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::noParkingAreasAsMarkerArray(
+  const std::vector<lanelet::NoParkingAreaConstPtr> & no_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  visualization_msgs::msg::Marker marker;
+
+  if (no_reg_elems.empty()) {
+    return marker_array;
+  }
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = false;
+  marker.ns = "no_parking_area";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker.lifetime = duration;
+  marker.pose.position.x = 0.0;  // p.x();
+  marker.pose.position.y = 0.0;  // p.y();
+  marker.pose.position.z = 0.0;  // p.z();
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.r = 1.0f;
+  marker.color.g = 1.0f;
+  marker.color.b = 1.0f;
+  marker.color.a = 0.999;
+
+  for (const auto & no_reg_elem : no_reg_elems) {
+    marker.points.clear();
+    marker.colors.clear();
+    marker.id = static_cast<int32_t>(no_reg_elem->id());
+
+    // area visualization
+    const auto no_parking_areas = no_reg_elem->noParkingAreas();
+    for (const auto & no_parking_area : no_parking_areas) {
+      geometry_msgs::msg::Polygon geom_poly;
+      utils::conversion::toGeomMsgPoly(no_parking_area, &geom_poly);
+
+      std::vector<geometry_msgs::msg::Polygon> triangles;
+      polygon2Triangle(geom_poly, &triangles);
+
+      for (auto tri : triangles) {
+        geometry_msgs::msg::Point tri0[3];
+
+        for (int i = 0; i < 3; i++) {
+          utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);
+          marker.points.push_back(tri0[i]);
+          marker.colors.push_back(c);
+        }
+      }  // for triangles0
+    }    // for no_parking areas
+    marker_array.markers.push_back(marker);
+  }  // for regulatory elements
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::noStoppingAreasAsMarkerArray(
+  const std::vector<lanelet::NoStoppingAreaConstPtr> & no_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  visualization_msgs::msg::Marker marker;
+  visualization_msgs::msg::Marker line_marker;
+
+  if (no_reg_elems.empty()) {
+    return marker_array;
+  }
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = false;
+  marker.ns = "no_stopping_area";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker.lifetime = duration;
+  marker.pose.position.x = 0.0;  // p.x();
+  marker.pose.position.y = 0.0;  // p.y();
+  marker.pose.position.z = 0.0;  // p.z();
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.r = 1.0f;
+  marker.color.g = 1.0f;
+  marker.color.b = 1.0f;
+  marker.color.a = 0.999;
+
+  std_msgs::msg::ColorRGBA line_c;
+  line_c.r = 0.5;
+  line_c.g = 0.5;
+  line_c.b = 0.5;
+  line_c.a = 0.999;
+  visualization::initLineStringMarker(&line_marker, "map", "no_stopping_area_stopline", line_c);
+
+  for (const auto & no_reg_elem : no_reg_elems) {
+    marker.points.clear();
+    marker.colors.clear();
+    marker.id = static_cast<int32_t>(no_reg_elem->id());
+
+    // area visualization
+    const auto no_stopping_areas = no_reg_elem->noStoppingAreas();
+    for (const auto & no_stopping_area : no_stopping_areas) {
+      geometry_msgs::msg::Polygon geom_poly;
+      utils::conversion::toGeomMsgPoly(no_stopping_area, &geom_poly);
+
+      std::vector<geometry_msgs::msg::Polygon> triangles;
+      polygon2Triangle(geom_poly, &triangles);
+
+      for (auto tri : triangles) {
+        geometry_msgs::msg::Point tri0[3];
+
+        for (int i = 0; i < 3; i++) {
+          utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);
+          marker.points.push_back(tri0[i]);
+          marker.colors.push_back(c);
+        }
+      }  // for triangles0
+    }    // for no_stopping areas
+    marker_array.markers.push_back(marker);
+    const auto & stop_line = no_reg_elem->stopLine();
+    // stop line visualization
+    if (stop_line) {
+      visualization::pushLineStringMarker(&line_marker, stop_line.value(), line_c, 0.5);
+    }
+  }  // for regulatory elements
+  if (!line_marker.points.empty()) {
+    marker_array.markers.push_back(line_marker);
+  }
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::speedBumpsAsMarkerArray(
+  const std::vector<lanelet::SpeedBumpConstPtr> & sb_reg_elems, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  visualization_msgs::msg::Marker marker;
+  visualization_msgs::msg::Marker line_marker;
+
+  if (sb_reg_elems.empty()) {
+    return marker_array;
+  }
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = false;
+  marker.ns = "speed_bump";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker.lifetime = duration;
+  marker.pose.position.x = 0.0;  // p.x();
+  marker.pose.position.y = 0.0;  // p.y();
+  marker.pose.position.z = 0.0;  // p.z();
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.r = 1.0f;
+  marker.color.g = 1.0f;
+  marker.color.b = 1.0f;
+  marker.color.a = 0.999;
+
+  for (const auto & sb_reg_elem : sb_reg_elems) {
+    marker.points.clear();
+    marker.colors.clear();
+    marker.id = static_cast<int32_t>(sb_reg_elem->id());
+
+    // area visualization
+    const auto speed_bump = sb_reg_elem->speedBump();
+
+    geometry_msgs::msg::Polygon geom_poly;
+    utils::conversion::toGeomMsgPoly(speed_bump, &geom_poly);
+
+    std::vector<geometry_msgs::msg::Polygon> triangles;
+    polygon2Triangle(geom_poly, &triangles);
+
+    for (auto tri : triangles) {
+      geometry_msgs::msg::Point tri0[3];
+
+      for (int i = 0; i < 3; i++) {
+        utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);
+        marker.points.push_back(tri0[i]);
+        marker.colors.push_back(c);
+      }
+    }  // for triangles0
+
+    marker_array.markers.push_back(marker);
+  }  // for regulatory elements
+
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::crosswalkAreasAsMarkerArray(
+  const std::vector<lanelet::CrosswalkConstPtr> & cw_reg_elems, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  visualization_msgs::msg::Marker marker;
+  visualization_msgs::msg::Marker line_marker;
+
+  if (cw_reg_elems.empty()) {
+    return marker_array;
+  }
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = false;
+  marker.ns = "crosswalk_areas";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker.lifetime = duration;
+  marker.pose.position.x = 0.0;  // p.x();
+  marker.pose.position.y = 0.0;  // p.y();
+  marker.pose.position.z = 0.0;  // p.z();
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.r = 0.8f;
+  marker.color.g = 0.8f;
+  marker.color.b = 0.0f;
+  marker.color.a = 0.999;
+
+  for (const auto & cw_reg_elem : cw_reg_elems) {
+    marker.points.clear();
+    marker.colors.clear();
+    marker.id = static_cast<int32_t>(cw_reg_elem->id());
+
+    // area visualization
+    const auto crosswalk_areas = cw_reg_elem->crosswalkAreas();
+    for (const auto & crosswalk_area : crosswalk_areas) {
+      geometry_msgs::msg::Polygon geom_poly;
+      utils::conversion::toGeomMsgPoly(crosswalk_area, &geom_poly);
+
+      std::vector<geometry_msgs::msg::Polygon> triangles;
+      polygon2Triangle(geom_poly, &triangles);
+
+      for (auto tri : triangles) {
+        geometry_msgs::msg::Point tri0[3];
+
+        for (int i = 0; i < 3; i++) {
+          utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);
+          marker.points.push_back(tri0[i]);
+          marker.colors.push_back(c);
+        }
+      }  // for triangles0
+    }
+    marker_array.markers.push_back(marker);
+  }
+
+  marker_array.markers.push_back(line_marker);
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::pedestrianPolygonMarkingsAsMarkerArray(
+  const lanelet::ConstLineStrings3d & pedestrian_polygon_markings,
+  const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (pedestrian_polygon_markings.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker polygon_marker =
+    createPolygonMarker("pedestrian_polygon_marking", c);
+  for (const auto & linestring : pedestrian_polygon_markings) {
+    lanelet::ConstPolygon3d polygon;
+    if (utils::lineStringToPolygon(linestring, &polygon)) {
+      pushPolygonMarker(&polygon_marker, polygon, c);
+    } else {
+      RCLCPP_WARN_STREAM(
+        rclcpp::get_logger("lanelet2_extension.visualization"),
+        "pedestrian marking " << linestring.id() << " failed conversion.");
+    }
+  }
+
+  if (!polygon_marker.points.empty()) {
+    marker_array.markers.push_back(polygon_marker);
+  }
+
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::pedestrianLineMarkingsAsMarkerArray(
+  const lanelet::ConstLineStrings3d & pedestrian_line_markings, const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (pedestrian_line_markings.empty()) {
+    return marker_array;
+  }
+
+  const float lss = 0.1;  // line string size
+  visualization_msgs::msg::Marker line_marker;
+  visualization::initLineStringMarker(&line_marker, "map", "pedestrian_line_marking", c);
+
+  for (const auto & linestring : pedestrian_line_markings) {
+    if ((linestring.size() < 3) && (linestring.front().id() != linestring.back().id())) {
+      pushLineStringMarker(&line_marker, linestring, c, lss);
+    }
+  }
+
+  if (!line_marker.points.empty()) {
+    marker_array.markers.push_back(line_marker);
+  }
+
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::parkingLotsAsMarkerArray(
+  const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (parking_lots.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker = createPolygonMarker("parking_lots", c);
+  for (const auto & polygon : parking_lots) {
+    pushPolygonMarker(&marker, polygon, c);
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray(
+  const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (parking_spaces.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker = createPolygonMarker("parking_space", c);
+  for (const auto & linestring : parking_spaces) {
+    lanelet::ConstPolygon3d polygon;
+    if (utils::lineStringWithWidthToPolygon(linestring, &polygon)) {
+      pushPolygonMarker(&marker, polygon, c);
+    } else {
+      std::cerr << "parking space " << linestring.id() << " failed conversion." << std::endl;
+    }
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::generateLaneletIdMarker(
+  const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c,
+  const std::string & ns, const double scale)
+{
+  visualization_msgs::msg::MarkerArray markers;
+  for (const auto & ll : road_lanelets) {
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = "map";
+    marker.header.stamp = rclcpp::Clock().now();
+    marker.ns = ns;
+    marker.id = static_cast<int32_t>(ll.id());
+    marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    const auto centerline = ll.centerline();
+    const size_t target_position_index = centerline.size() / 2;
+    const auto & target_position = centerline[target_position_index];
+    marker.pose.position.x = target_position.x();
+    marker.pose.position.y = target_position.y();
+    marker.pose.position.z = target_position.z();
+    marker.pose.orientation.x = 0.0;
+    marker.pose.orientation.y = 0.0;
+    marker.pose.orientation.z = 0.0;
+    marker.pose.orientation.w = 1.0;
+    marker.color = c;
+    marker.scale.z = scale;
+    marker.frame_locked = false;
+    marker.text = std::to_string(ll.id());
+    markers.markers.push_back(marker);
+  }
+  return markers;
+}
+
+visualization_msgs::msg::MarkerArray visualization::obstaclePolygonsAsMarkerArray(
+  const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (obstacle_polygons.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker = createPolygonMarker("obstacles", c);
+  for (const auto & polygon : obstacle_polygons) {
+    pushPolygonMarker(&marker, polygon, c);
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::lineStringsAsMarkerArray(
+  const std::vector<lanelet::ConstLineString3d> & line_strings, const std::string & name_space,
+  const std_msgs::msg::ColorRGBA & c, const float lss)
+{
+  visualization_msgs::msg::MarkerArray ls_marker_array;
+  if (line_strings.empty()) {
+    return ls_marker_array;
+  }
+  std::unordered_set<lanelet::Id> added;
+  visualization_msgs::msg::Marker ls_marker;
+  visualization::initLineStringMarker(&ls_marker, "map", name_space, c);
+
+  for (const auto & ls : line_strings) {
+    if (!exists(added, ls.id())) {
+      visualization::pushLineStringMarker(&ls_marker, ls, c, lss);
+      added.insert(ls.id());
+    }
+  }
+  ls_marker_array.markers.push_back(ls_marker);
+  return ls_marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArray(
+  const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c,
+  const bool viz_centerline, const std::string & additional_namespace)
+{
+  const float lss = 0.1;  // line string size
+  const float lss_center = static_cast<float>(std::max(lss * 0.1, 0.02));
+
+  std::unordered_set<lanelet::Id> added;
+  visualization_msgs::msg::Marker left_line_strip;
+  visualization_msgs::msg::Marker right_line_strip;
+  visualization_msgs::msg::Marker start_bound_line_strip;
+  visualization_msgs::msg::Marker center_line_strip;
+  visualization_msgs::msg::Marker center_arrows;
+  visualization::initLineStringMarker(
+    &left_line_strip, "map", additional_namespace + "left_lane_bound", c);
+  visualization::initLineStringMarker(
+    &right_line_strip, "map", additional_namespace + "right_lane_bound", c);
+  visualization::initLineStringMarker(
+    &start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c);
+  visualization::initLineStringMarker(
+    &center_line_strip, "map", additional_namespace + "center_lane_line", c);
+  visualization::initArrowsMarker(
+    &center_arrows, "map", additional_namespace + "center_line_arrows", c);
+
+  for (const auto & lll : lanelets) {
+    lanelet::ConstLineString3d left_ls = lll.leftBound();
+    lanelet::ConstLineString3d right_ls = lll.rightBound();
+    lanelet::ConstLineString3d center_ls = lll.centerline();
+    lanelet::LineString3d start_bound_ls(lanelet::utils::getId());
+    start_bound_ls.push_back(lanelet::Point3d(
+      lanelet::utils::getId(), left_ls.front().x(), left_ls.front().y(), left_ls.front().z()));
+    start_bound_ls.push_back(lanelet::Point3d(
+      lanelet::utils::getId(), right_ls.front().x(), right_ls.front().y(), right_ls.front().z()));
+
+    if (!exists(added, left_ls.id())) {
+      visualization::pushLineStringMarker(&left_line_strip, left_ls, c, lss);
+      added.insert(left_ls.id());
+    }
+    if (!exists(added, right_ls.id())) {
+      visualization::pushLineStringMarker(&right_line_strip, right_ls, c, lss);
+      added.insert(right_ls.id());
+    }
+    if (!exists(added, start_bound_ls.id())) {
+      visualization::pushLineStringMarker(&start_bound_line_strip, start_bound_ls, c, lss);
+      added.insert(start_bound_ls.id());
+    }
+    if (viz_centerline && !exists(added, center_ls.id())) {
+      visualization::pushLineStringMarker(&center_line_strip, center_ls, c, lss_center);
+      visualization::pushArrowsMarker(&center_arrows, center_ls, c);
+      added.insert(center_ls.id());
+    }
+  }
+
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (!left_line_strip.points.empty()) {
+    marker_array.markers.push_back(left_line_strip);
+  }
+  if (!right_line_strip.points.empty()) {
+    marker_array.markers.push_back(right_line_strip);
+  }
+  if (!center_line_strip.points.empty()) {
+    marker_array.markers.push_back(center_line_strip);
+  }
+  if (!start_bound_line_strip.points.empty()) {
+    marker_array.markers.push_back(start_bound_line_strip);
+  }
+  if (!center_arrows.points.empty()) {
+    marker_array.markers.push_back(center_arrows);
+  }
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::trafficLightsAsTriangleMarkerArray(
+  const std::vector<lanelet::TrafficLightConstPtr> & tl_reg_elems,
+  const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale)
+{
+  // convert to to an array of linestrings and publish as marker array using
+  // existing function
+
+  std::vector<lanelet::ConstLineString3d> line_strings;
+  visualization_msgs::msg::Marker marker;
+  visualization::initTrafficLightTriangleMarker(&marker, "traffic_light_triangle", duration);
+
+  for (const auto & tl : tl_reg_elems) {
+    lanelet::LineString3d ls;
+
+    const auto lights = tl->trafficLights();
+    for (const auto & lsp : lights) {
+      if (lsp.isLineString()) {  // traffic lights can either polygons or linestrings
+        lanelet::ConstLineString3d ls = static_cast<lanelet::ConstLineString3d>(lsp);
+        visualization::pushTrafficLightTriangleMarker(&marker, ls, c, scale);
+      }
+    }
+  }
+
+  visualization_msgs::msg::MarkerArray marker_array;
+  marker_array.markers.push_back(marker);
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::laneletsAsTriangleMarkerArray(
+  const std::string & ns, const lanelet::ConstLanelets & lanelets,
+  const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  visualization_msgs::msg::Marker marker;
+
+  if (lanelets.empty()) {
+    return marker_array;
+  }
+
+  marker.header.frame_id = "map";
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = false;
+  marker.ns = ns;
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker.lifetime = rclcpp::Duration(0, 0);
+  marker.pose.position.x = 0.0;  // p.x();
+  marker.pose.position.y = 0.0;  // p.y();
+  marker.pose.position.z = 0.0;  // p.z();
+  marker.pose.orientation.x = 0.0;
+  marker.pose.orientation.y = 0.0;
+  marker.pose.orientation.z = 0.0;
+  marker.pose.orientation.w = 1.0;
+  marker.scale.x = 1.0;
+  marker.scale.y = 1.0;
+  marker.scale.z = 1.0;
+  marker.color.r = 1.0f;
+  marker.color.g = 1.0f;
+  marker.color.b = 1.0f;
+  marker.color.a = 0.999;
+
+  for (const auto & ll : lanelets) {
+    std::vector<geometry_msgs::msg::Polygon> triangles;
+    lanelet2Triangle(ll, &triangles);
+
+    for (const auto & tri : triangles) {
+      geometry_msgs::msg::Point tri0[3];
+
+      for (int i = 0; i < 3; i++) {
+        utils::conversion::toGeomMsgPt(tri.points[i], &tri0[i]);
+
+        marker.points.push_back(tri0[i]);
+        marker.colors.push_back(c);
+      }
+    }
+  }
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+
+void visualization::initTrafficLightTriangleMarker(
+  visualization_msgs::msg::Marker * marker, const std::string & ns,
+  const rclcpp::Duration & duration)
+{
+  if (marker == nullptr) {
+    std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl;
+    return;
+  }
+
+  marker->header.frame_id = "map";
+  marker->header.stamp = rclcpp::Time();
+  marker->frame_locked = false;
+  marker->ns = ns;
+  marker->id = 0;
+  marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+  marker->lifetime = duration;
+
+  marker->pose.position.x = 0.0;  // p.x();
+  marker->pose.position.y = 0.0;  // p.y();
+  marker->pose.position.z = 0.0;  // p.z();
+  marker->pose.orientation.x = 0.0;
+  marker->pose.orientation.y = 0.0;
+  marker->pose.orientation.z = 0.0;
+  marker->pose.orientation.w = 1.0;
+  marker->scale.x = 1.0;
+  marker->scale.y = 1.0;
+  marker->scale.z = 1.0;
+  marker->color.r = 1.0f;
+  marker->color.g = 1.0f;
+  marker->color.b = 1.0f;
+  marker->color.a = 0.999;
+}
+
+void visualization::pushTrafficLightTriangleMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
+  const std_msgs::msg::ColorRGBA & cl, const double scale)
+{
+  if (marker == nullptr) {
+    std::cerr << __FUNCTION__ << ": marker is null pointer!" << std::endl;
+    return;
+  }
+
+  double h = 0.7;
+  if (ls.hasAttribute("height")) {
+    lanelet::Attribute attr = ls.attribute("height");
+    h = std::stod(attr.value());
+  }
+
+  // construct triangles and add to marker
+
+  // define polygon of traffic light border
+  Eigen::Vector3d v[4];
+  v[0] << ls.front().x(), ls.front().y(), ls.front().z();
+  v[1] << ls.back().x(), ls.back().y(), ls.back().z();
+  v[2] << ls.back().x(), ls.back().y(), ls.back().z() + h;
+  v[3] << ls.front().x(), ls.front().y(), ls.front().z() + h;
+
+  Eigen::Vector3d c = (v[0] + v[1] + v[2] + v[3]) / 4;
+
+  if (scale > 0.0 && scale != 1.0) {
+    for (auto & i : v) {
+      i = (i - c) * scale + c;
+    }
+  }
+  geometry_msgs::msg::Point tri0[3];
+  utils::conversion::toGeomMsgPt(v[0], &tri0[0]);
+  utils::conversion::toGeomMsgPt(v[1], &tri0[1]);
+  utils::conversion::toGeomMsgPt(v[2], &tri0[2]);
+  geometry_msgs::msg::Point tri1[3];
+  utils::conversion::toGeomMsgPt(v[0], &tri1[0]);
+  utils::conversion::toGeomMsgPt(v[2], &tri1[1]);
+  utils::conversion::toGeomMsgPt(v[3], &tri1[2]);
+
+  for (const auto & i : tri0) {
+    marker->points.push_back(i);
+    marker->colors.push_back(cl);
+  }
+  for (const auto & i : tri1) {
+    marker->points.push_back(i);
+    marker->colors.push_back(cl);
+  }
+}
+
+void visualization::initLineStringMarker(
+  visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns,
+  const std_msgs::msg::ColorRGBA & c)
+{
+  if (marker == nullptr) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker is null pointer!");
+    return;
+  }
+
+  marker->header.frame_id = frame_id;
+  marker->header.stamp = rclcpp::Time();
+  marker->frame_locked = false;
+  marker->ns = ns;
+  marker->action = visualization_msgs::msg::Marker::ADD;
+  marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+
+  marker->id = 0;
+  marker->pose.orientation.x = 0.0;
+  marker->pose.orientation.y = 0.0;
+  marker->pose.orientation.z = 0.0;
+  marker->pose.orientation.w = 1.0;
+  marker->scale.x = 1.0;
+  marker->scale.y = 1.0;
+  marker->scale.z = 1.0;
+  marker->color = c;
+}
+
+void visualization::pushLineStringMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
+  const std_msgs::msg::ColorRGBA & c, const float lss)
+{
+  if (marker == nullptr) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker is null pointer!");
+    return;
+  }
+
+  // fill out lane line
+  if (ls.size() < 2) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker line size is 1 or 0!");
+    return;
+  }
+  for (auto i = ls.begin(); i + 1 != ls.end(); i++) {
+    geometry_msgs::msg::Point p;
+    const auto heading =
+      static_cast<float>(std::atan2((*(i + 1)).y() - (*i).y(), (*(i + 1)).x() - (*i).x()));
+
+    const auto x_offset = static_cast<float>(lss * 0.5 * std::sin(heading));
+    const auto y_offset = static_cast<float>(lss * 0.5 * std::cos(heading));
+
+    p.x = (*i).x() + x_offset;
+    p.y = (*i).y() - y_offset;
+    p.z = (*i).z();
+    marker->points.push_back(p);
+    p.x = (*i).x() - x_offset;
+    p.y = (*i).y() + y_offset;
+    p.z = (*i).z();
+    marker->points.push_back(p);
+    p.x = (*(i + 1)).x() + x_offset;
+    p.y = (*(i + 1)).y() - y_offset;
+    p.z = (*(i + 1)).z();
+    marker->points.push_back(p);
+    marker->colors.push_back(c);
+    p.x = (*(i + 1)).x() - x_offset;
+    p.y = (*(i + 1)).y() + y_offset;
+    p.z = (*(i + 1)).z();
+    marker->points.push_back(p);
+    p.x = (*(i + 1)).x() + x_offset;
+    p.y = (*(i + 1)).y() - y_offset;
+    p.z = (*(i + 1)).z();
+    marker->points.push_back(p);
+    p.x = (*i).x() - x_offset;
+    p.y = (*i).y() + y_offset;
+    p.z = (*i).z();
+    marker->points.push_back(p);
+    marker->colors.push_back(c);
+  }
+}
+
+void visualization::initArrowsMarker(
+  visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns,
+  const std_msgs::msg::ColorRGBA & c)
+{
+  if (marker == nullptr) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker is null pointer!");
+    return;
+  }
+
+  marker->header.frame_id = frame_id;
+  marker->header.stamp = rclcpp::Time();
+  marker->frame_locked = false;
+  marker->ns = ns;
+  marker->action = visualization_msgs::msg::Marker::ADD;
+  marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST;
+
+  marker->id = 0;
+  marker->pose.orientation.x = 0.0;
+  marker->pose.orientation.y = 0.0;
+  marker->pose.orientation.z = 0.0;
+  marker->pose.orientation.w = 1.0;
+  marker->scale.x = 1.0;
+  marker->scale.y = 1.0;
+  marker->scale.z = 1.0;
+  marker->color = c;
+}
+
+void visualization::pushArrowsMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
+  const std_msgs::msg::ColorRGBA & c)
+{
+  if (marker == nullptr) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker is null pointer!");
+    return;
+  }
+
+  // fill out lane line
+  if (ls.size() < 2) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker line size is 1 or 0!");
+    return;
+  }
+  for (auto i = ls.begin(); i + 1 != ls.end(); i++) {
+    const auto heading =
+      static_cast<float>(std::atan2((*(i + 1)).y() - (*i).y(), (*(i + 1)).x() - (*i).x()));
+
+    const float sin_offset = std::sin(heading);
+    const float cos_offset = std::cos(heading);
+    const double width = 0.3;
+    const double height = 1.0;
+
+    geometry_msgs::msg::Point p;
+    p.x = (*i).x() + sin_offset * width;
+    p.y = (*i).y() - cos_offset * width;
+    p.z = (*i).z();
+    marker->points.push_back(p);
+    p.x = (*i).x() - sin_offset * width;
+    p.y = (*i).y() + cos_offset * width;
+    p.z = (*i).z();
+    marker->points.push_back(p);
+    p.x = (*i).x() + cos_offset * height;
+    p.y = (*i).y() + sin_offset * height;
+    p.z = (*i).z();
+    marker->points.push_back(p);
+    marker->colors.push_back(c);
+  }
+}
+
+visualization_msgs::msg::MarkerArray visualization::intersectionAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & intersection_areas, const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (intersection_areas.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker = createPolygonMarker("intersection_area", c);
+  for (const auto & polygon : intersection_areas) {
+    pushPolygonMarker(&marker, polygon, c);
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::noObstacleSegmentationAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_obstacle_segmentation_area,
+  const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (no_obstacle_segmentation_area.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker = createPolygonMarker("no_obstacle_segmentation_area", c);
+  for (const auto & polygon : no_obstacle_segmentation_area) {
+    pushPolygonMarker(&marker, polygon, c);
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray
+visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray(
+  const lanelet::ConstPolygons3d & no_obstacle_segmentation_area_for_run_out,
+  const std_msgs::msg::ColorRGBA & c)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (no_obstacle_segmentation_area_for_run_out.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker marker =
+    createPolygonMarker("no_obstacle_segmentation_area_for_run_out", c);
+  for (const auto & polygon : no_obstacle_segmentation_area_for_run_out) {
+    pushPolygonMarker(&marker, polygon, c);
+  }
+
+  if (!marker.points.empty()) {
+    marker_array.markers.push_back(marker);
+  }
+  return marker_array;
+}
+
+visualization_msgs::msg::MarkerArray visualization::hatchedRoadMarkingsAreaAsMarkerArray(
+  const lanelet::ConstPolygons3d & hatched_road_markings_area,
+  const std_msgs::msg::ColorRGBA & area_color, const std_msgs::msg::ColorRGBA & line_color)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (hatched_road_markings_area.empty()) {
+    return marker_array;
+  }
+
+  // polygon
+  visualization_msgs::msg::Marker area_marker =
+    createPolygonMarker("hatched_road_markings_area", area_color);
+  for (const auto & polygon : hatched_road_markings_area) {
+    pushPolygonMarker(&area_marker, polygon, area_color);
+  }
+
+  if (!area_marker.points.empty()) {
+    marker_array.markers.push_back(area_marker);
+  }
+
+  // line strings
+  const float lss = 0.1;  // line string size
+  visualization_msgs::msg::Marker line_strip;
+  visualization::initLineStringMarker(
+    &line_strip, "map", "hatched_road_markings_bound", line_color);
+
+  for (const auto & polygon : hatched_road_markings_area) {
+    lanelet::LineString3d bound_ls(lanelet::utils::getId());
+    for (const auto & point : polygon) {
+      bound_ls.push_back(
+        lanelet::Point3d(lanelet::utils::getId(), point.x(), point.y(), point.z()));
+    }
+    if (!bound_ls.empty()) {
+      bound_ls.push_back(bound_ls.front());
+    }
+    visualization::pushLineStringMarker(&line_strip, bound_ls, line_color, lss);
+  }
+  if (!line_strip.points.empty()) {
+    marker_array.markers.push_back(line_strip);
+  }
+
+  return marker_array;
+}
+}  // namespace lanelet
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/package.xml b/lanelet2_extension/package.xml
new file mode 100644
index 0000000..2f697db
--- /dev/null
+++ b/lanelet2_extension/package.xml
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lanelet2_extension</name>
+  <version>0.1.0</version>
+  <description>The lanelet2_extension package contains libraries to handle Lanelet2 format data.</description>
+  <maintainer email="ryohsuke.mitsudome@tier4.jp">mitsudome-r</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+  <maintainer email="yutaka.kondo@tier4.jp">Yutaka Kondo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>geographiclib</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_core</depend>
+  <depend>lanelet2_io</depend>
+  <depend>lanelet2_maps</depend>
+  <depend>lanelet2_projection</depend>
+  <depend>lanelet2_routing</depend>
+  <depend>lanelet2_traffic_rules</depend>
+  <depend>lanelet2_validation</depend>
+  <depend>pugixml-dev</depend>
+  <depend>range-v3</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/lanelet2_extension/src/check_right_of_way.cpp b/lanelet2_extension/src/check_right_of_way.cpp
new file mode 100644
index 0000000..8b2a8be
--- /dev/null
+++ b/lanelet2_extension/src/check_right_of_way.cpp
@@ -0,0 +1,97 @@
+// Copyright 2015-2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+#include <range/v3/algorithm/move.hpp>
+#include <range/v3/core.hpp>
+#include <range/v3/functional/overload.hpp>
+#include <range/v3/utility/copy.hpp>
+#include <range/v3/view/filter.hpp>
+#include <range/v3/view/transform.hpp>
+#include <range/v3/view/unique.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <algorithm>
+#include <iostream>
+#include <memory>
+#include <set>
+#include <utility>
+#include <vector>
+
+int main(int argc, char ** argv)
+{
+  if (argc < 2) {
+    std::cout << "usage: ros2 run lanelet2_extension check_right_of_way <map_path>";
+    return 0;
+  }
+
+  // NOLINTBEGIN
+  const std::string map_path = std::string(argv[1]);
+  // NOLINTEND
+
+  lanelet::ErrorMessages errors{};
+  lanelet::projection::MGRSProjector projector{};
+  lanelet::LaneletMapPtr map = lanelet::load(map_path, projector, &errors);
+  for (auto && error : errors) std::cout << error << std::endl;
+
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
+    lanelet::traffic_rules::TrafficRulesFactory::create(
+      lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  lanelet::routing::RoutingGraphPtr routing_graph_ptr =
+    lanelet::routing::RoutingGraph::build(*map, *traffic_rules);
+
+  auto rows = map->regulatoryElementLayer  // filter elem whose Subtype is RightOfWay
+              |
+              ranges::views::filter([](auto && elem) {
+                const auto & attrs = elem->attributes();
+                auto it = attrs.find(lanelet::AttributeName::Subtype);
+                return it != attrs.end() && it->second == lanelet::AttributeValueString::RightOfWay;
+              })  // transform to lanelet::RightOfWay
+              | ranges::views::transform([](auto && elem) {
+                  return std::move(std::dynamic_pointer_cast<lanelet::RightOfWay>(elem));
+                });
+
+  for (auto && row : rows) {
+    const auto & right_of_ways = row->rightOfWayLanelets();
+    const auto & yields = row->yieldLanelets();
+    std::set<lanelet::Id> yield_ids;
+    for (auto && yield : yields) {
+      yield_ids.insert(yield.id());
+    }
+
+    std::set<lanelet::Id> conflicting_ids;
+    for (auto && right_of_way : right_of_ways) {
+      const std::vector<lanelet::ConstLanelet> & conflicting_lanelets =
+        lanelet::utils::getConflictingLanelets(routing_graph_ptr, right_of_way);
+      for (auto && conflict : conflicting_lanelets) conflicting_ids.insert(conflict.id());
+    }
+
+    std::vector<lanelet::Id> unnecessary_yields;
+    set_difference(
+      yield_ids.begin(), yield_ids.end(), conflicting_ids.begin(), conflicting_ids.end(),
+      std::inserter(unnecessary_yields, unnecessary_yields.end()));
+    if (unnecessary_yields.empty()) continue;
+    std::cout << "RightOfWay " << row->id() << ": [";
+    const char * delim = "";
+    for (auto && unnecessary_yield : unnecessary_yields)
+      std::cout << std::exchange(delim, ",") << unnecessary_yield;
+    std::cout << "] are unnecessary yield" << std::endl;
+  }
+}
diff --git a/lanelet2_extension/src/sample_code.cpp b/lanelet2_extension/src/sample_code.cpp
new file mode 100644
index 0000000..d4595fb
--- /dev/null
+++ b/lanelet2_extension/src/sample_code.cpp
@@ -0,0 +1,125 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/projection/mgrs_projector.hpp"
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_projection/UTM.h>
+
+#include <iostream>
+#include <string>
+
+void loadingAutowareOSMFile(const std::string & map_file_path)
+{
+  lanelet::LaneletMapPtr lanelet_map;
+  lanelet::ErrorMessages errors;
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 49.0;
+  gps_point.lon = 8.4;
+  lanelet::Origin origin(gps_point);
+  lanelet::projection::UtmProjector projector(lanelet::Origin(gps_point), true, false);
+
+  // Autoware OSM file parser is registered into lanelet2 library.
+  // Therefore, you can used it by just specifying "autoware_osm_handler" parser
+  // in load function.
+  lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors);
+
+  // If you want to use default parser, explicitly name the parser
+  lanelet_map = lanelet::load(map_file_path, "osm_handler", projector, &errors);
+}
+
+void usingMGRSProjector()
+{
+  // MGRS Projector projects lat/lon to x,y,z point in MGRS 100km grid.
+  // The origin is automatically calculated so you don't have to select any
+  // origin.
+  lanelet::projection::MGRSProjector projector;
+
+  // Let's convert lat/lng point into mgrs xyz point.
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 49.0;
+  gps_point.lon = 8.4;
+  gps_point.ele = 0.0;
+
+  lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point);
+  std::cout << mgrs_point << std::endl;
+
+  // You can get MGRS Code of projected grid.
+  std::string mgrs_grid = projector.getProjectedMGRSGrid();
+  std::cout << mgrs_grid << std::endl;
+
+  // You can also reverse project from MGRS point to lat/lon.
+  // You have to set which MGRS grid it is from or it will reuse last projected
+  // grid
+  lanelet::GPSPoint projected_gps_point = projector.reverse(mgrs_point);
+  std::cout << projected_gps_point.lat << " " << projected_gps_point.lon << std::endl;
+  lanelet::GPSPoint projected_gps_point2 =
+    lanelet::projection::MGRSProjector::reverse(mgrs_point, mgrs_grid);
+  std::cout << projected_gps_point2.lat << " " << projected_gps_point2.lon << " " << std::endl;
+}
+
+void usingAutowareTrafficLight(const std::string & map_file_path)
+{
+  lanelet::LaneletMapPtr lanelet_map;
+  lanelet::ErrorMessages errors;
+  lanelet::projection::MGRSProjector projector;
+  lanelet_map = lanelet::load(map_file_path, "autoware_osm_handler", projector, &errors);
+
+  for (const auto & lanelet : lanelet_map->laneletLayer) {
+    // You can access to traffic light element as AutowareTrafficLight class
+    const auto autoware_traffic_lights =
+      lanelet.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>();
+    for (const auto & light : autoware_traffic_lights) {
+      // You can access to the position of each lamps(light bulb) in traffic
+      // light
+      for (const auto & light_bulb_string : light->lightBulbs()) {
+        std::cout << light_bulb_string.id() << std::endl;
+      }
+      // Since AutowareTrafficLight class is inheriting lanelet::TrafficLight
+      // class, you can also access to outline of traffic light by the same
+      // method.
+      for (const auto & light_string : light->trafficLights()) {
+        std::cout << light_string.id() << std::endl;
+      }
+    }
+
+    // You can also access to same traffic light element as default TrafficLight
+    // class
+    auto traffic_lights = lanelet.regulatoryElementsAs<lanelet::TrafficLight>();
+    for (const auto & light : traffic_lights) {
+      for (const auto & light_string : light->trafficLights()) {
+        std::cout << light_string.id() << std::endl;
+      }
+    }
+  }
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("sample_code");
+  const std::string map_file_path = node->declare_parameter("map_file", "");
+  loadingAutowareOSMFile(map_file_path);
+  usingMGRSProjector();
+  usingAutowareTrafficLight(map_file_path);
+  return 0;
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/src/validation.cpp b/lanelet2_extension/src/validation.cpp
new file mode 100644
index 0000000..4b08714
--- /dev/null
+++ b/lanelet2_extension/src/validation.cpp
@@ -0,0 +1,179 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/projection/mgrs_projector.hpp"
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+
+#include <pugixml.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_io/io_handlers/Factory.h>
+#include <lanelet2_io/io_handlers/Writer.h>
+#include <lanelet2_projection/UTM.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <iostream>
+#include <string>
+
+namespace
+{
+namespace keyword
+{
+constexpr const char * Id = "id";
+constexpr const char * Osm = "osm";
+constexpr const char * Tag = "tag";
+constexpr const char * Key = "k";
+constexpr const char * Node = "node";
+constexpr const char * Elevation = "ele";
+}  // namespace keyword
+
+void printUsage()
+{
+  std::cout << "Usage:" << std::endl
+            << "ros2 run lanelet2_extension autoware_lanelet2_validation"
+               " --ros-args -p map_file:=<path to osm file>"
+            << std::endl;
+}
+}  // namespace
+
+void validateElevationTag(const std::string & filename)
+{
+  pugi::xml_document doc;
+  auto result = doc.load_file(filename.c_str());
+  if (!result) {
+    std::cerr << result.description() << std::endl;
+    exit(1);
+  }
+
+  auto osmNode = doc.child(keyword::Osm);
+  for (auto node = osmNode.child(keyword::Node); node; node = node.next_sibling(keyword::Node)) {
+    const auto id = node.attribute(keyword::Id).as_llong(lanelet::InvalId);
+    if (!node.find_child_by_attribute(keyword::Tag, keyword::Key, keyword::Elevation)) {
+      std::cerr << "failed to find elevation tag for node: " << id << std::endl;
+    }
+  }
+}
+
+// NOLINTBEGIN(readability-function-cognitive-complexity)
+void validateTrafficLight(const lanelet::LaneletMapPtr lanelet_map)
+{
+  if (!lanelet_map) {
+    std::cerr << "Missing map. Are you sure you set correct path for map?" << std::endl;
+    exit(1);
+  }
+
+  for (const auto & lanelet : lanelet_map->laneletLayer) {
+    const auto autoware_traffic_lights =
+      lanelet.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>();
+    if (autoware_traffic_lights.empty()) {
+      continue;
+    }
+    for (const auto & light : autoware_traffic_lights) {
+      if (light->lightBulbs().empty()) {
+        std::cerr << "regulatory element traffic light " << light->id()
+                  << " is missing optional light_bulb member. You won't be able to use region_tlr "
+                     "node with this map"
+                  << std::endl;
+      }
+      for (const auto & light_string : light->lightBulbs()) {
+        if (!light_string.hasAttribute("traffic_light_id")) {
+          std::cerr << "light_bulb " << light_string.id() << " is missing traffic_light_id tag"
+                    << std::endl;
+        }
+      }
+      for (const auto & base_string_or_poly : light->trafficLights()) {
+        if (!base_string_or_poly.isLineString()) {
+          std::cerr
+            << "traffic_light " << base_string_or_poly.id()
+            << " is polygon, and only linestring class is currently supported for traffic lights"
+            << std::endl;
+        }
+        const auto base_string = static_cast<lanelet::ConstLineString3d>(base_string_or_poly);
+        if (!base_string.hasAttribute("height")) {
+          std::cerr << "traffic_light " << base_string.id() << " is missing height tag"
+                    << std::endl;
+        }
+      }
+    }
+  }
+}
+// NOLINTEND(readability-function-cognitive-complexity)
+
+void validateTurnDirection(const lanelet::LaneletMapPtr lanelet_map)
+{
+  if (!lanelet_map) {
+    std::cerr << "Missing map. Are you sure you set correct path for map?" << std::endl;
+    exit(1);
+  }
+
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
+    lanelet::traffic_rules::TrafficRulesFactory::create(
+      lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  lanelet::routing::RoutingGraphPtr vehicle_graph =
+    lanelet::routing::RoutingGraph::build(*lanelet_map, *traffic_rules);
+
+  for (const auto & lanelet : lanelet_map->laneletLayer) {
+    if (!traffic_rules->canPass(lanelet)) {
+      continue;
+    }
+
+    const auto conflicting_lanelets_or_areas = vehicle_graph->conflicting(lanelet);
+    if (conflicting_lanelets_or_areas.empty()) {
+      continue;
+    }
+    if (!lanelet.hasAttribute("turn_direction")) {
+      std::cerr
+        << "lanelet " << lanelet.id()
+        << " seems to be intersecting other lanelet, but does not have turn_direction tagging."
+        << std::endl;
+    }
+  }
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  auto node = rclcpp::Node::make_shared("autoware_lanelet_validation");
+
+  std::string map_path;
+  try {
+    map_path = node->declare_parameter<std::string>("map_file");
+  } catch (...) {
+    std::cerr << "failed find map_file parameter! No file to load" << std::endl;
+    printUsage();
+    return 1;
+  }
+  lanelet::LaneletMapPtr lanelet_map;
+  lanelet::ErrorMessages errors;
+  lanelet::projection::MGRSProjector projector;
+  lanelet_map = lanelet::load(map_path, "autoware_osm_handler", projector, &errors);
+
+  std::cout << "starting validation" << std::endl;
+
+  validateElevationTag(map_path);
+  validateTrafficLight(lanelet_map);
+  validateTurnDirection(lanelet_map);
+
+  std::cout << "finished validation" << std::endl;
+
+  return 0;
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/test/src/test_message_conversion.cpp b/lanelet2_extension/test/src/test_message_conversion.cpp
new file mode 100644
index 0000000..4f31ab5
--- /dev/null
+++ b/lanelet2_extension/test/src/test_message_conversion.cpp
@@ -0,0 +1,175 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/utility/message_conversion.hpp"
+#include "lanelet2_extension/utility/query.hpp"
+
+#include <gtest/gtest.h>
+#include <lanelet2_core/LaneletMap.h>
+
+#include <cmath>
+
+using lanelet::Lanelet;
+using lanelet::LineString3d;
+using lanelet::Point3d;
+using lanelet::utils::getId;
+using lanelet::utils::conversion::toGeomMsgPt;
+
+class TestSuite : public ::testing::Test  // NOLINT for gtest
+{
+public:
+  TestSuite() : single_lanelet_map_ptr(new lanelet::LaneletMap())
+  {
+    Point3d p1;
+    Point3d p2;
+    Point3d p3;
+    Point3d p4;
+    Point3d p5;
+    Point3d p6;
+    Point3d p7;
+    LineString3d traffic_light_base;
+    LineString3d traffic_light_bulbs;
+    LineString3d stop_line;
+
+    p1 = Point3d(getId(), 0., 0., 0.);
+    p2 = Point3d(getId(), 0., 1., 0.);
+
+    p3 = Point3d(getId(), 1., 0., 0.);
+    p4 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left(getId(), {p1, p2});
+    LineString3d ls_right(getId(), {p3, p4});
+
+    Lanelet lanelet(getId(), ls_left, ls_right);
+
+    single_lanelet_map_ptr->add(lanelet);
+  }
+
+  ~TestSuite() override = default;
+
+  lanelet::LaneletMapPtr single_lanelet_map_ptr;
+
+private:
+};
+
+TEST_F(TestSuite, BinMsgConversion)  // NOLINT for gtest
+{
+  autoware_auto_mapping_msgs::msg::HADMapBin bin_msg;
+  lanelet::LaneletMapPtr regenerated_map(new lanelet::LaneletMap);
+
+  lanelet::utils::conversion::toBinMsg(single_lanelet_map_ptr, &bin_msg);
+
+  ASSERT_NE(0U, bin_msg.data.size()) << "converted bin message does not have any data";
+
+  lanelet::utils::conversion::fromBinMsg(bin_msg, regenerated_map);
+
+  auto original_lanelet = lanelet::utils::query::laneletLayer(single_lanelet_map_ptr);
+  auto regenerated_lanelet = lanelet::utils::query::laneletLayer(regenerated_map);
+
+  ASSERT_EQ(original_lanelet.front().id(), regenerated_lanelet.front().id())
+    << "regenerated map has different id";
+}
+
+TEST_F(TestSuite, ToGeomMsgPt)  // NOLINT for gtest
+{
+  Point3d lanelet_pt(getId(), -0.1, 0.2, 3.0);
+
+  geometry_msgs::msg::Point32 geom_pt32;
+  geom_pt32.x = -0.1;
+  geom_pt32.y = 0.2;
+  geom_pt32.z = 3.0;
+
+  geometry_msgs::msg::Point geom_pt;
+  toGeomMsgPt(geom_pt32, &geom_pt);
+  ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x)
+    << " converted value is different from original geometry_msgs::msg::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y)
+    << " converted value is different from original geometry_msgs::msg::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z)
+    << " converted value is different from original geometry_msgs::msg::Point";
+
+  geom_pt = toGeomMsgPt(geom_pt32);
+  ASSERT_FLOAT_EQ(geom_pt32.x, geom_pt.x)
+    << " converted value is different from original geometry_msgs::msg::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.y, geom_pt.y)
+    << " converted value is different from original geometry_msgs::msg::Point";
+  ASSERT_FLOAT_EQ(geom_pt32.z, geom_pt.z)
+    << " converted value is different from original geometry_msgs::msg::Point";
+
+  toGeomMsgPt(lanelet_pt.basicPoint(), &geom_pt);
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x)
+    << " converted value is different from original "
+       "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y)
+    << " converted value is different from original "
+       "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z)
+    << " converted value is different from original "
+       "lanelet::basicPoint";
+
+  geom_pt = toGeomMsgPt(lanelet_pt.basicPoint());
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().x(), geom_pt.x)
+    << " converted value is different from original "
+       "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().y(), geom_pt.y)
+    << " converted value is different from original "
+       "lanelet::basicPoint";
+  ASSERT_DOUBLE_EQ(lanelet_pt.basicPoint().z(), geom_pt.z)
+    << " converted value is different from original "
+       "lanelet::basicPoint";
+
+  toGeomMsgPt(lanelet_pt, &geom_pt);
+  ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x)
+    << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y)
+    << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z)
+    << " converted value is different from original lanelet::Point3d";
+
+  geom_pt = toGeomMsgPt(lanelet_pt);
+  ASSERT_DOUBLE_EQ(lanelet_pt.x(), geom_pt.x)
+    << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.y(), geom_pt.y)
+    << " converted value is different from original lanelet::Point3d";
+  ASSERT_DOUBLE_EQ(lanelet_pt.z(), geom_pt.z)
+    << " converted value is different from original lanelet::Point3d";
+
+  lanelet::ConstPoint2d point_2d = lanelet::utils::to2D(lanelet_pt);
+
+  toGeomMsgPt(point_2d, &geom_pt);
+  ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x)
+    << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y)
+    << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(0.0, geom_pt.z)
+    << " converted value is different from original lanelet::Point2d";
+
+  geom_pt = toGeomMsgPt(point_2d);
+  ASSERT_DOUBLE_EQ(point_2d.x(), geom_pt.x)
+    << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(point_2d.y(), geom_pt.y)
+    << " converted value is different from original lanelet::Point2d";
+  ASSERT_DOUBLE_EQ(0.0, geom_pt.z)
+    << " converted value is different from original lanelet::Point2d";
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/test/src/test_projector.cpp b/lanelet2_extension/test/src/test_projector.cpp
new file mode 100644
index 0000000..3f3e7b8
--- /dev/null
+++ b/lanelet2_extension/test/src/test_projector.cpp
@@ -0,0 +1,159 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/projection/mgrs_projector.hpp"
+#include "lanelet2_extension/projection/transverse_mercator_projector.hpp"
+
+#include <gtest/gtest.h>
+#include <lanelet2_io/Projection.h>
+
+#include <cmath>
+
+class TestSuite : public ::testing::Test  // NOLINT for gtest
+{
+public:
+  TestSuite() = default;
+  ~TestSuite() override = default;
+};
+
+TEST(TestSuite, ForwardMGRSProjection)  // NOLINT for gtest
+{
+  lanelet::projection::MGRSProjector projector;
+  // lat/lon in Tokyo
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 35.652832;
+  gps_point.lon = 139.839478;
+  gps_point.ele = 12.3456789;
+  lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point);
+
+  // projected z value should not change
+  ASSERT_DOUBLE_EQ(mgrs_point.z(), gps_point.ele)
+    << "Forward projected z value should be " << gps_point.ele;
+
+  // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html
+  // round the projected value to mm since the above reference only gives value
+  // in mm precision
+  ASSERT_EQ(projector.getProjectedMGRSGrid(), "54SUE") << "Projected grid should be "
+                                                       << "54SUE";
+  double rounded_x_mm = round(mgrs_point.x() * 1000) / 1000.0;
+  ASSERT_DOUBLE_EQ(rounded_x_mm, 94946.081) << "Forward projected x value should be " << 94946.081;
+  double rounded_y_mm = round(mgrs_point.y() * 1000) / 1000.0;
+  ASSERT_DOUBLE_EQ(rounded_y_mm, 46063.748) << "Forward projected y value should be " << 46063.748;
+}
+
+TEST(TestSuite, ReverseMGRSProjection)  // NOLINT for gtest
+{
+  lanelet::projection::MGRSProjector projector;
+  lanelet::BasicPoint3d mgrs_point;
+  mgrs_point.x() = 94946.0;
+  mgrs_point.y() = 46063.0;
+  mgrs_point.z() = 12.3456789;
+
+  projector.setMGRSCode("54SUE");
+  lanelet::GPSPoint gps_point = projector.reverse(mgrs_point);
+
+  // projected z value should not change
+  ASSERT_DOUBLE_EQ(gps_point.ele, mgrs_point.z())
+    << "Reverse projected z value should be " << mgrs_point.z();
+
+  // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html
+  // round the projected value since the above reference only gives value up to
+  // precision of 1e-8
+  double rounded_lat = round(gps_point.lat * 1e8) / 1e8;
+  ASSERT_DOUBLE_EQ(rounded_lat, 35.65282525)
+    << "Reverse projected latitude value should be " << 35.65282525;
+  double rounded_lon = round(gps_point.lon * 1e8) / 1e8;
+  ASSERT_DOUBLE_EQ(rounded_lon, 139.83947721)
+    << "Reverse projected longitude value should be " << 139.83947721;
+}
+
+TEST(TestSuite, ForwardTransverseMercatorProjection)  // NOLINT for gtest
+{
+  lanelet::projection::TransverseMercatorProjector projector(
+    lanelet::Origin({35.652832, 139.839478}));
+
+  // lat/lon in Tokyo
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 35.652832;
+  gps_point.lon = 139.839478;
+  gps_point.ele = 12.3456789;
+  lanelet::BasicPoint3d projected_point = projector.forward(gps_point);
+
+  // projected z value should not change
+  ASSERT_DOUBLE_EQ(projected_point.z(), gps_point.ele)
+    << "Forward projected z value should be " << gps_point.ele;
+
+  double rounded_x_mm = round(projected_point.x() * 1000) / 1000.0;
+  ASSERT_DOUBLE_EQ(rounded_x_mm, 0.0) << "Forward projected x value should be " << 0.0;
+  double rounded_y_mm = round(projected_point.y() * 1000) / 1000.0;
+  ASSERT_DOUBLE_EQ(rounded_y_mm, 0.0) << "Forward projected y value should be " << 0.0;
+}
+
+TEST(TestSuite, ReverseTransverseMercatorProjection)  // NOLINT for gtest
+{
+  lanelet::projection::TransverseMercatorProjector projector(
+    lanelet::Origin({35.652832, 139.839478}));
+
+  lanelet::BasicPoint3d local_point;
+  local_point.x() = 0.0;
+  local_point.y() = 0.0;
+  local_point.z() = 12.3456789;
+
+  lanelet::GPSPoint gps_point = projector.reverse(local_point);
+
+  // projected z value should not change
+  ASSERT_DOUBLE_EQ(gps_point.ele, local_point.z())
+    << "Reverse projected z value should be " << local_point.z();
+
+  // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html
+  // round the projected value since the above reference only gives value up to
+  // precision of 1e-8
+  double rounded_lat = round(gps_point.lat * 1e8) / 1e8;
+  ASSERT_DOUBLE_EQ(rounded_lat, 35.652832)
+    << "Reverse projected latitude value should be " << 35.652832;
+  double rounded_lon = round(gps_point.lon * 1e8) / 1e8;
+  ASSERT_DOUBLE_EQ(rounded_lon, 139.839478)
+    << "Reverse projected longitude value should be " << 139.839478;
+}
+
+TEST(TestSuite, ForwardAndReverseTransverseMercatorProjection)  // NOLINT for gtest
+{
+  lanelet::projection::TransverseMercatorProjector projector(lanelet::Origin({35.0, 139.0}));
+
+  // lat/lon in Tokyo
+  lanelet::GPSPoint gps_point;
+  gps_point.lat = 35.652832;
+  gps_point.lon = 139.839478;
+  gps_point.ele = 12.3456789;
+  lanelet::BasicPoint3d projected_point = projector.forward(gps_point);
+  lanelet::GPSPoint gps_point_converted = projector.reverse(projected_point);
+
+  // projected z value should not change
+  ASSERT_DOUBLE_EQ(gps_point_converted.ele, gps_point.ele)
+    << "Forward projected z value should be " << gps_point.ele;
+  EXPECT_NEAR(gps_point_converted.lat, gps_point.lat, 0.0001)
+    << "Forward and Reversed latitude should match the original latitude";
+  EXPECT_NEAR(gps_point_converted.lon, gps_point.lon, 0.0001)
+    << "Forward and Reversed longitude should match the original longitude";
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/test/src/test_query.cpp b/lanelet2_extension/test/src/test_query.cpp
new file mode 100644
index 0000000..3fa73cf
--- /dev/null
+++ b/lanelet2_extension/test/src/test_query.cpp
@@ -0,0 +1,149 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+#include "lanelet2_extension/utility/query.hpp"
+
+#include <gtest/gtest.h>
+#include <lanelet2_core/LaneletMap.h>
+
+#include <cmath>
+
+using lanelet::Lanelet;
+using lanelet::LineString3d;
+using lanelet::Point3d;
+using lanelet::Points3d;
+using lanelet::utils::getId;
+
+class TestSuite : public ::testing::Test  // NOLINT for gtest
+{
+public:
+  TestSuite() : sample_map_ptr(new lanelet::LaneletMap())
+  {
+    // create sample lanelets
+    Point3d p1;
+    Point3d p2;
+    Point3d p3;
+    Point3d p4;
+
+    p1 = Point3d(getId(), 0., 0., 0.);
+    p2 = Point3d(getId(), 0., 1., 0.);
+
+    p3 = Point3d(getId(), 1., 0., 0.);
+    p4 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left(getId(), {p1, p2});
+    LineString3d ls_right(getId(), {p3, p4});
+
+    Lanelet road_lanelet(getId(), ls_left, ls_right);
+    road_lanelet.attributes()[lanelet::AttributeName::Subtype] =
+      lanelet::AttributeValueString::Road;
+
+    Lanelet crosswalk_lanelet(getId(), ls_left, ls_right);
+    crosswalk_lanelet.attributes()[lanelet::AttributeName::Subtype] =
+      lanelet::AttributeValueString::Crosswalk;
+
+    // create sample traffic light
+    Point3d p5;
+    Point3d p6;
+    Point3d p7;
+    Point3d p8;
+    Point3d p9;
+    Point3d p10;
+    Point3d p11;
+    Point3d p12;
+    LineString3d traffic_light_base;
+    LineString3d traffic_light_bulbs;
+    LineString3d stop_line;
+
+    p6 = Point3d(getId(), 0., 1., 4.);
+    p7 = Point3d(getId(), 1., 1., 4.);
+
+    p8 = Point3d(getId(), 0., 1., 4.5);
+    p9 = Point3d(getId(), 0.5, 1., 4.5);
+    p10 = Point3d(getId(), 1., 1., 4.5);
+
+    p11 = Point3d(getId(), 0., 0., 0.);
+    p12 = Point3d(getId(), 1., 0., 0.);
+
+    traffic_light_base = LineString3d(getId(), Points3d{p6, p7});
+    traffic_light_bulbs = LineString3d(getId(), Points3d{p8, p9, p10});
+    stop_line = LineString3d(getId(), Points3d{p11, p12});
+
+    auto tl = lanelet::autoware::AutowareTrafficLight::make(
+      getId(), lanelet::AttributeMap(), {traffic_light_base}, stop_line, {traffic_light_bulbs});
+
+    road_lanelet.addRegulatoryElement(tl);
+
+    // add items to map
+    sample_map_ptr->add(road_lanelet);
+    sample_map_ptr->add(crosswalk_lanelet);
+  }
+
+  ~TestSuite() override = default;
+
+  lanelet::LaneletMapPtr sample_map_ptr;
+
+private:
+};
+
+TEST_F(TestSuite, QueryLanelets)  // NOLINT for gtest
+{
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr);
+  ASSERT_EQ(2U, all_lanelets.size()) << "failed to retrieve all lanelets";
+
+  lanelet::ConstLanelets subtype_lanelets =
+    lanelet::utils::query::subtypeLanelets(all_lanelets, lanelet::AttributeValueString::Road);
+  ASSERT_EQ(1U, subtype_lanelets.size()) << "failed to retrieve road lanelet by subtypeLanelets";
+
+  lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets);
+  ASSERT_EQ(1U, road_lanelets.size()) << "failed to retrieve road lanelets";
+
+  lanelet::ConstLanelets crosswalk_lanelets =
+    lanelet::utils::query::crosswalkLanelets(all_lanelets);
+  ASSERT_EQ(1U, crosswalk_lanelets.size()) << "failed to retrieve crosswalk lanelets";
+}
+
+TEST_F(TestSuite, QueryTrafficLights)  // NOLINT for gtest
+{
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr);
+
+  auto traffic_lights = lanelet::utils::query::trafficLights(all_lanelets);
+  ASSERT_EQ(1U, traffic_lights.size()) << "failed to retrieve traffic lights";
+
+  auto autoware_traffic_lights = lanelet::utils::query::autowareTrafficLights(all_lanelets);
+  ASSERT_EQ(1U, autoware_traffic_lights.size()) << "failed to retrieve autoware traffic lights";
+}
+
+TEST_F(TestSuite, QueryStopLine)  // NOLINT for gtest
+{
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(sample_map_ptr);
+  lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets);
+
+  auto stop_lines = lanelet::utils::query::stopLinesLanelets(all_lanelets);
+  ASSERT_EQ(1U, stop_lines.size()) << "failed to retrieve stop lines from all lanelets";
+
+  auto stop_lines2 = lanelet::utils::query::stopLinesLanelet(road_lanelets.front());
+  ASSERT_EQ(1U, stop_lines2.size()) << "failed to retrieve stop lines from a lanelet";
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/test/src/test_regulatory_elements.cpp b/lanelet2_extension/test/src/test_regulatory_elements.cpp
new file mode 100644
index 0000000..7037e95
--- /dev/null
+++ b/lanelet2_extension/test/src/test_regulatory_elements.cpp
@@ -0,0 +1,145 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
+
+#include <gtest/gtest.h>
+#include <lanelet2_core/Attribute.h>
+#include <lanelet2_core/LaneletMap.h>
+
+#include <cmath>
+#include <vector>
+
+using lanelet::LineString3d;
+using lanelet::LineStringOrPolygon3d;
+using lanelet::Point3d;
+using lanelet::Points3d;
+using lanelet::utils::getId;
+
+namespace
+{
+template <typename T>
+std::vector<T> convertToVector(T item)
+{
+  std::vector<T> vector = {item};
+  return vector;
+}
+}  // namespace
+
+class TestSuite : public ::testing::Test  // NOLINT for gtest
+{
+public:
+  TestSuite() = default;
+  ~TestSuite() override = default;
+};
+
+TEST(TestSuite, FactoryConstructsTrafficLight)  // NOLINT for gtest
+{
+  Point3d p1;
+  Point3d p2;
+  Point3d p3;
+  Point3d p4;
+  Point3d p5;
+  Point3d p6;
+  Point3d p7;
+  LineStringOrPolygon3d traffic_light_base;
+  LineString3d traffic_light_bulbs;
+  LineString3d stop_line;
+
+  p1 = Point3d(getId(), 0., 1., 4.);
+  p2 = Point3d(getId(), 1., 1., 4.);
+
+  p3 = Point3d(getId(), 0., 1., 4.5);
+  p4 = Point3d(getId(), 0.5, 1., 4.5);
+  p5 = Point3d(getId(), 1., 1., 4.5);
+
+  p6 = Point3d(getId(), 0., 0., 0.);
+  p7 = Point3d(getId(), 1., 0., 0.);
+
+  Points3d base = {p1, p2};
+  Points3d bulbs = {p3, p4, p5};
+  Points3d stop = {p6, p7};
+
+  traffic_light_base = LineString3d(getId(), base);
+  traffic_light_bulbs = LineString3d(getId(), bulbs);
+  stop_line = LineString3d(getId(), stop);
+
+  auto tl = lanelet::autoware::AutowareTrafficLight::make(
+    getId(), lanelet::AttributeMap(), convertToVector(traffic_light_base), stop_line,
+    convertToVector(traffic_light_bulbs));
+
+  auto factoryTl = lanelet::RegulatoryElementFactory::create(
+    tl->attribute(lanelet::AttributeName::Subtype).value(),
+    std::const_pointer_cast<lanelet::RegulatoryElementData>(tl->constData()));
+  EXPECT_TRUE(!!std::dynamic_pointer_cast<lanelet::TrafficLight>(factoryTl));
+}
+
+TEST(TestSuite, TrafficLightWorksAsExpected)  // NOLINT for gtest
+{
+  Point3d p1;
+  Point3d p2;
+  Point3d p3;
+  Point3d p4;
+  Point3d p5;
+  Point3d p6;
+  Point3d p7;
+
+  LineStringOrPolygon3d traffic_light_base;
+  LineStringOrPolygon3d traffic_light_base2;
+  LineString3d traffic_light_bulbs;
+  LineString3d traffic_light_bulbs2;
+  LineString3d stop_line;
+
+  p1 = Point3d(getId(), 0., 1., 4.);
+  p2 = Point3d(getId(), 1., 1., 4.);
+
+  p3 = Point3d(getId(), 0., 1., 4.5);
+  p4 = Point3d(getId(), 0.5, 1., 4.5);
+  p5 = Point3d(getId(), 1., 1., 4.5);
+
+  p6 = Point3d(getId(), 0., 0., 0.);
+  p7 = Point3d(getId(), 1., 0., 0.);
+
+  Points3d base = {p1, p2};
+  Points3d bulbs = {p3, p4, p5};
+  Points3d stop = {p6, p7};
+
+  traffic_light_base = {LineString3d(getId(), base)};
+  traffic_light_base2 = {LineString3d(getId(), base)};
+  traffic_light_bulbs = {LineString3d(getId(), bulbs)};
+  traffic_light_bulbs2 = {LineString3d(getId(), bulbs)};
+  stop_line = LineString3d(getId(), stop);
+
+  auto tl = lanelet::autoware::AutowareTrafficLight::make(
+    getId(), lanelet::AttributeMap(), convertToVector(traffic_light_base), stop_line,
+    convertToVector(traffic_light_bulbs));
+  tl->setStopLine(stop_line);
+  EXPECT_EQ(stop_line, tl->stopLine());
+  tl->addTrafficLight(traffic_light_base2);
+  EXPECT_EQ(2ul, tl->trafficLights().size());
+  tl->addLightBulbs(traffic_light_bulbs2);
+  EXPECT_EQ(2ul, tl->lightBulbs().size());
+  tl->removeLightBulbs(traffic_light_bulbs);
+  EXPECT_EQ(1ul, tl->lightBulbs().size());
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/test/src/test_route_checker.cpp b/lanelet2_extension/test/src/test_route_checker.cpp
new file mode 100644
index 0000000..660d33e
--- /dev/null
+++ b/lanelet2_extension/test/src/test_route_checker.cpp
@@ -0,0 +1,100 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include "lanelet2_extension/utility/message_conversion.hpp"
+#include "lanelet2_extension/utility/route_checker.hpp"
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+
+#include <gtest/gtest.h>
+#include <lanelet2_core/LaneletMap.h>
+
+using lanelet::Lanelet;
+using lanelet::LineString3d;
+using lanelet::Point3d;
+using lanelet::utils::getId;
+
+class TestSuite : public ::testing::Test  // NOLINT for gtest
+{
+public:
+  TestSuite() : sample_map_ptr(new lanelet::LaneletMap())
+  {
+    // create sample lanelets
+    const Point3d p1(getId(), 0.0, 0.0, 0.0);
+    const Point3d p2(getId(), 0.0, 1.0, 0.0);
+
+    const LineString3d ls_left(getId(), {p1, p2});
+
+    const Point3d p3(getId(), 1.0, 0.0, 0.0);
+    const Point3d p4(getId(), 1.0, 0.0, 0.0);
+
+    const LineString3d ls_right(getId(), {p3, p4});
+
+    const Lanelet lanelet(getId(), ls_left, ls_right);
+
+    sample_map_ptr->add(lanelet);
+
+    // create sample routes
+    autoware_planning_msgs::msg::LaneletPrimitive map_primitive;
+    autoware_planning_msgs::msg::LaneletSegment map_segment1;
+    autoware_planning_msgs::msg::LaneletSegment map_segment2;
+
+    for (size_t i = 0; i < 2; i++) {
+      map_primitive.id = lanelet.id();
+      map_segment1.primitives.push_back(map_primitive);
+      map_primitive.id = ls_left.id();
+      map_segment2.primitives.push_back(map_primitive);
+    }
+    sample_route1.segments.push_back(map_segment1);
+    sample_route2.segments.push_back(map_segment2);
+  }
+
+  ~TestSuite() override = default;
+
+  lanelet::LaneletMapPtr sample_map_ptr;
+  autoware_planning_msgs::msg::LaneletRoute sample_route1;  // valid route
+  autoware_planning_msgs::msg::LaneletRoute sample_route2;  // invalid route
+
+private:
+};
+
+TEST_F(TestSuite, isRouteValid)  // NOLINT for gtest
+{
+  autoware_auto_mapping_msgs::msg::HADMapBin bin_msg;
+
+  const auto route_ptr1 =
+    std::make_shared<autoware_planning_msgs::msg::LaneletRoute>(sample_route1);
+  const auto route_ptr2 =
+    std::make_shared<autoware_planning_msgs::msg::LaneletRoute>(sample_route2);
+
+  // toBinMsg is tested at test_message_conversion.cpp
+  lanelet::utils::conversion::toBinMsg(sample_map_ptr, &bin_msg);
+
+  ASSERT_TRUE(lanelet::utils::route::isRouteValid(*route_ptr1, sample_map_ptr))
+    << "The route should be valid, which should be created on the same map as the current one";
+  ASSERT_FALSE(lanelet::utils::route::isRouteValid(*route_ptr2, sample_map_ptr))
+    << "The route should be invalid, which should be created on the different map from the current "
+       "one";
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension/test/src/test_utilities.cpp b/lanelet2_extension/test/src/test_utilities.cpp
new file mode 100644
index 0000000..4d53c03
--- /dev/null
+++ b/lanelet2_extension/test/src/test_utilities.cpp
@@ -0,0 +1,167 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTBEGIN(readability-identifier-naming, cppcoreguidelines-avoid-goto)
+
+#include "lanelet2_extension/utility/utilities.hpp"
+
+#include <gtest/gtest.h>
+#include <lanelet2_core/primitives/Point.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <map>
+
+using lanelet::Lanelet;
+using lanelet::LineString3d;
+using lanelet::Point3d;
+using lanelet::utils::getId;
+
+class TestSuite : public ::testing::Test  // NOLINT for gtest
+{
+public:
+  TestSuite() : sample_map_ptr(new lanelet::LaneletMap())
+  {
+    // create sample lanelets
+    Point3d p1;
+    Point3d p2;
+    Point3d p3;
+    Point3d p4;
+    Point3d p5;
+    Point3d p6;
+    Point3d p7;
+    Point3d p8;
+    Point3d p9;
+    Point3d p10;
+
+    p1 = Point3d(getId(), 0., 0., 0.);
+    p2 = Point3d(getId(), 0., 1., 0.);
+    p3 = Point3d(getId(), 1., 0., 0.);
+    p4 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left(getId(), {p1, p2});
+    LineString3d ls_right(getId(), {p3, p4});
+
+    p5 = Point3d(getId(), 0., 2., 0.);
+    p6 = Point3d(getId(), 1., 2., 0.);
+
+    LineString3d ls_left2(getId(), {p2, p5});
+    LineString3d ls_right2(getId(), {p4, p6});
+
+    p7 = Point3d(getId(), 0., 3., 0.);
+    p8 = Point3d(getId(), 1., 3., 0.);
+
+    LineString3d ls_left3(getId(), {p5, p7});
+    LineString3d ls_right3(getId(), {p6, p8});
+
+    p9 = Point3d(getId(), 0., 1., 0.);
+    p10 = Point3d(getId(), 1., 1., 0.);
+
+    LineString3d ls_left4(getId(), {p9, p5});
+    LineString3d ls_right4(getId(), {p10, p6});
+
+    road_lanelet = Lanelet(getId(), ls_left, ls_right);
+    road_lanelet.attributes()[lanelet::AttributeName::Subtype] =
+      lanelet::AttributeValueString::Road;
+
+    next_lanelet = Lanelet(getId(), ls_left2, ls_right2);
+    next_lanelet.attributes()[lanelet::AttributeName::Subtype] =
+      lanelet::AttributeValueString::Road;
+
+    next_lanelet2 = Lanelet(getId(), ls_left3, ls_right3);
+    next_lanelet2.attributes()[lanelet::AttributeName::Subtype] =
+      lanelet::AttributeValueString::Road;
+
+    merging_lanelet = Lanelet(getId(), ls_left4, ls_right4);
+    merging_lanelet.attributes()[lanelet::AttributeName::Subtype] =
+      lanelet::AttributeValueString::Road;
+
+    sample_map_ptr->add(road_lanelet);
+    sample_map_ptr->add(next_lanelet);
+    sample_map_ptr->add(next_lanelet2);
+    sample_map_ptr->add(merging_lanelet);
+  }
+
+  ~TestSuite() override = default;
+
+  lanelet::LaneletMapPtr sample_map_ptr;
+  Lanelet road_lanelet;
+  Lanelet next_lanelet;
+  Lanelet next_lanelet2;
+  Lanelet merging_lanelet;
+
+private:
+};
+
+TEST_F(TestSuite, OverwriteLaneletsCenterline)  // NOLINT for gtest
+{
+  double resolution = 5.0;
+  bool force_overwrite = false;
+  lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr, resolution, force_overwrite);
+
+  for (const auto & lanelet : sample_map_ptr->laneletLayer) {
+    ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline";
+  }
+}
+
+/*
+TEST(Utilities, copyZ)  // NOLINT for gtest
+{
+  using lanelet::utils::copyZ;
+
+  LineString3d ls1;
+  LineString3d ls2;
+
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  ls1.push_back(Point3d(lanelet::InvalId, 0.0, 0.0, 5.0));
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  ls2.push_back(Point3d(lanelet::InvalId, 0.0, 0.0, 0.0));
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  EXPECT_EQ(ls2.front().z(), ls1.front().z());
+  ls1.push_back(Point3d(lanelet::InvalId, 1.0, 0.0, 2.0));
+  ls2.push_back(Point3d(lanelet::InvalId, 3.0, 0.0, 0.0));
+  ASSERT_NO_THROW(copyZ(ls1, ls2));
+  EXPECT_EQ(ls2.front().z(), ls1.front().z());
+  EXPECT_EQ(ls2.back().z(), ls1.back().z());
+
+  ls1.push_back(Point3d(lanelet::InvalId, 2.0, 0.0, 0.0));
+  ls1.push_back(Point3d(lanelet::InvalId, 3.0, 0.0, 3.0));
+  lanelet::Points3d points;
+  points.emplace_back(lanelet::InvalId, 0.0, 0.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 0.5, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 1.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 1.5, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 2.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 2.5, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 3.0, 0.0);
+  points.emplace_back(lanelet::InvalId, 0.0, 3.5, 0.0);
+  ASSERT_NO_THROW(copyZ(ls1, points));
+  EXPECT_EQ(points[0].z(), 5.0);
+  EXPECT_EQ(points[1].z(), 3.5);
+  EXPECT_EQ(points[2].z(), 2.0);
+  EXPECT_EQ(points[3].z(), 1.0);
+  EXPECT_EQ(points[4].z(), 0.0);
+  EXPECT_EQ(points[5].z(), 1.5);
+  EXPECT_EQ(points[6].z(), 3.0);
+  EXPECT_EQ(points[7].z(), 3.0);
+}
+*/
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+// NOLINTEND(readability-identifier-naming, cppcoreguidelines-avoid-goto)
diff --git a/lanelet2_extension_python/CMakeLists.txt b/lanelet2_extension_python/CMakeLists.txt
new file mode 100644
index 0000000..2b9e028
--- /dev/null
+++ b/lanelet2_extension_python/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.14)
+project(lanelet2_extension_python)
+
+find_package(autoware_cmake REQUIRED)
+find_package(python_cmake_module REQUIRED)
+
+autoware_package()
+
+find_package(BoostPython REQUIRED)
+
+include_directories(
+  ${BoostPython_INCLUDE_DIRS}
+
+)
+ament_python_install_package(${PROJECT_NAME})
+
+set(CMAKE_SHARED_LIBRARY_PREFIX "")
+ament_auto_add_library(_${PROJECT_NAME}_boost_python_utility SHARED
+  src/utility.cpp
+)
+ament_auto_add_library(_${PROJECT_NAME}_boost_python_projection SHARED
+  src/projection.cpp
+)
+ament_auto_add_library(_${PROJECT_NAME}_boost_python_regulatory_elements SHARED
+  src/regulatory_elements.cpp
+)
+install(TARGETS _${PROJECT_NAME}_boost_python_utility _${PROJECT_NAME}_boost_python_projection _${PROJECT_NAME}_boost_python_regulatory_elements
+  DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
+)
+
+ament_package()
diff --git a/lanelet2_extension_python/example/example.py b/lanelet2_extension_python/example/example.py
new file mode 100644
index 0000000..c9311bc
--- /dev/null
+++ b/lanelet2_extension_python/example/example.py
@@ -0,0 +1,195 @@
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Pose
+import lanelet2
+import lanelet2.geometry
+from lanelet2_extension_python.projection import MGRSProjector
+import lanelet2_extension_python.utility.query as query
+import lanelet2_extension_python.utility.utilities as utilities
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+def test_projection():
+    return MGRSProjector(lanelet2.io.Origin(0.0, 0.0))
+
+
+def test_io(map_path, projection):
+    return lanelet2.io.load(map_path, projection)
+
+
+def plot_ll2_id(ll2, ax, text):
+    xs, ys = np.array([pt.x for pt in ll2.centerline]), np.array([pt.y for pt in ll2.centerline])
+    x, y = np.average(xs), np.average(ys)
+    ax.text(x, y, text)
+
+
+def plot_linestring(linestring, ax, color, linestyle, label, **kwargs):
+    xs = [pt.x for pt in linestring]
+    ys = [pt.y for pt in linestring]
+    ax.plot(xs, ys, color=color, linestyle=linestyle, label=label, **kwargs)
+
+
+def test_utility_utilities(lanelet_map, routing_graph):
+    lanelet108 = lanelet_map.laneletLayer.get(108)
+    lanelet108_next = query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0)
+    lanelet108_seq = [lanelet108, *lanelet108_next[0]]
+    lanelet108_seq_combined = utilities.combineLaneletsShape(lanelet108_seq)
+    lanelet108_seq_combined_fine_centerline = utilities.generateFineCenterline(
+        lanelet108_seq_combined, 1.0
+    )
+    lanelet108_seq_combined_right_offset = utilities.getRightBoundWithOffset(
+        lanelet108_seq_combined, 1.0, 1.0
+    )
+    lanelet108_seq_combined_left_offset = utilities.getLeftBoundWithOffset(
+        lanelet108_seq_combined, 1.0, 1.0
+    )
+    fig = plt.figure()
+    ax = fig.add_subplot(1, 3, 1)
+    ax.axis("equal")
+    plot_linestring(lanelet108_seq_combined.leftBound, ax, "orange", "-", "108 left")
+    plot_linestring(lanelet108_seq_combined.rightBound, ax, "orange", "-", "108 right")
+    plot_linestring(lanelet108_seq_combined_fine_centerline, ax, "orange", "--", "108 center")
+    plot_linestring(lanelet108_seq_combined_right_offset, ax, "cyan", "--", "108 right offset")
+    plot_linestring(lanelet108_seq_combined_left_offset, ax, "red", "--", "108 left offset")
+    [plot_ll2_id(ll2, ax, str(ll2.id)) for ll2 in lanelet108_seq]
+    ax.set_title("test of combineLaneletsShape ~ getLeftBoundWithOffset")
+
+    expanded_lanelet108 = utilities.getExpandedLanelet(lanelet108, 1.0, -1.0)
+    expanded_lanelet108_seq = utilities.getExpandedLanelets(lanelet108_seq, 2.0, -2.0)
+    ax = fig.add_subplot(1, 3, 2)
+    ax.axis("equal")
+    plot_linestring(expanded_lanelet108.leftBound, ax, "orange", "-", "expanded 108 left(1.0)")
+    plot_linestring(expanded_lanelet108.rightBound, ax, "orange", "-", "expanded 108 right(-1.0)")
+    plot_linestring(expanded_lanelet108.centerline, ax, "orange", "--", "expanded 108 center")
+    [
+        (
+            plot_linestring(ll2.leftBound, ax, "orange", "-", None),
+            plot_linestring(ll2.rightBound, ax, "orange", "-", None),
+        )
+        for ll2 in expanded_lanelet108_seq
+    ]
+    [plot_ll2_id(ll2, ax, f"{ll2.id}(2.0)") for ll2 in expanded_lanelet108_seq]
+    ax.set_title("test of getExpandedLanelet(s)")
+
+    print(f"""{utilities.getLaneletLength2d(lanelet108)=}""")
+    print(f"""{utilities.getLaneletLength2d(lanelet108_seq)=}""")
+    print(f"""{utilities.getLaneletLength3d(lanelet108)=}""")
+    print(f"""{utilities.getLaneletLength3d(lanelet108_seq)=}""")
+
+    search_pose = Pose(position=Point(x=3685.0, y=73750.0))
+    arc_coords = utilities.getArcCoordinates(lanelet108_seq, search_pose)
+    print(
+        f"""utilities.getArcCoordinates(lanelet108_seq, search_pose) = (length: {arc_coords.length}, distance: {arc_coords.distance})"""
+    )
+    closest_lanelet = query.getClosestLanelet(lanelet108_seq, search_pose)
+    assert closest_lanelet.id == 156
+    closest_segment = utilities.getClosestSegment(
+        lanelet2.core.BasicPoint2d(x=search_pose.position.x, y=search_pose.position.y),
+        closest_lanelet.centerline,
+    )
+    ax = fig.add_subplot(1, 3, 3)
+    ax.axis("equal")
+    plot_linestring(closest_lanelet.leftBound, ax, "orange", "--", "156 left")
+    plot_linestring(closest_lanelet.rightBound, ax, "orange", "--", "156 right")
+    plot_linestring(closest_lanelet.centerline, ax, "cyan", "--", "156 center")
+    plot_linestring(closest_segment, ax, "red", "-", "closest segment", linewidth=2)
+    ax.scatter([search_pose.position.x], [search_pose.position.y], marker="o", label="search_pose")
+
+    print(f"""{utilities.getLaneletAngle(closest_lanelet, search_pose.position)=}""")
+    print(f"""{utilities.isInLanelet(search_pose, closest_lanelet)=}""")
+    print(f"""{utilities.isInLanelet(search_pose, closest_lanelet, 5.0)=}""")
+    closest_center_pose = utilities.getClosestCenterPose(closest_lanelet, search_pose.position)
+    ax.scatter(
+        [closest_center_pose.position.x],
+        [closest_center_pose.position.y],
+        marker="o",
+        label="closest_center_pose",
+    )
+    print(f"""{utilities.getLateralDistanceToCenterline(closest_lanelet, search_pose)=}""")
+    print(f"""{utilities.getLateralDistanceToClosestLanelet(lanelet108_seq, search_pose)=}""")
+    plt.legend()
+    plt.show()
+
+
+def test_utility_query(lanelet_map, routing_graph):
+    lanelets = query.laneletLayer(lanelet_map)
+    lanelet56 = lanelet_map.laneletLayer.get(56)
+    lanelet108 = lanelet_map.laneletLayer.get(108)
+    print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""")
+    print(f"""{len(query.subtypeLanelets(lanelets, "road"))=}""")
+    print(f"""{len(query.crosswalkLanelets(lanelets))=}""")
+    print(f"""{len(query.walkwayLanelets(lanelets))=}""")
+    print(f"""{len(query.roadLanelets(lanelets))=}""")
+    print(f"""{len(query.shoulderLanelets(lanelets))=}""")
+    print(f"""{len(query.trafficLights(lanelets))=}""")
+    print(f"""{len(query.autowareTrafficLights(lanelets))=}""")
+    print(f"""{len(query.detectionAreas(lanelets))=}""")
+    print(f"""{len(query.noStoppingAreas(lanelets))=}""")
+    print(f"""{len(query.noParkingAreas(lanelets))=}""")
+    print(f"""{len(query.speedBumps(lanelets))=}""")
+    print(f"""{len(query.crosswalks(lanelets))=}""")
+    print(f"""{len(query.curbstones(lanelet_map))=}""")
+    print(f"""{len(query.getAllPolygonsByType(lanelet_map, "parking_lot"))=}""")
+    print(f"""{len(query.getAllParkingLots(lanelet_map))=}""")
+    print(f"""{len(query.getAllPartitions(lanelet_map))=}""")
+    print(f"""{len(query.getAllParkingSpaces(lanelet_map))=}""")
+    print(f"""{len(query.getAllFences(lanelet_map))=}""")
+    print(f"""{len(query.getAllPedestrianPolygonMarkings(lanelet_map))=}""")
+    print(f"""{len(query.getAllPedestrianLineMarkings(lanelet_map))=}""")
+    print(f"""{len(query.stopLinesLanelets(lanelets))=}""")
+    print(f"""{len(query.stopLinesLanelet(lanelet108))=}""")
+    print(f"""{len(query.stopSignStopLines(lanelets))=}""")
+    lanelet56_centerline_center = np.sum([p.basicPoint() for p in lanelet56.centerline]) * (
+        1.0 / len(lanelet56.centerline)
+    )
+    search_point = Point(
+        x=lanelet56_centerline_center.x,
+        y=lanelet56_centerline_center.y,
+        z=lanelet56_centerline_center.z,
+    )
+    print(f"""{search_point=}""")
+    print(f"""{[ll2.id for ll2 in query.getLaneletsWithinRange(lanelets, search_point, 15.0)]=}""")
+    search_point_2d = lanelet2.core.BasicPoint2d(
+        x=lanelet56_centerline_center.x, y=lanelet56_centerline_center.y
+    )
+    print(
+        f"""{[ll2.id for ll2 in query.getLaneletsWithinRange(lanelets, search_point_2d, 15.0)]=}"""
+    )
+    print(f"""{[ll2.id for ll2 in query.getLaneChangeableNeighbors(routing_graph, lanelet108)]=}""")
+    print(
+        f"""{[ll2.id for ll2 in query.getLaneChangeableNeighbors(routing_graph, query.roadLanelets(lanelets), search_point)]=}"""
+    )
+    print(f"""{[ll2.id for ll2 in query.getAllNeighbors(routing_graph, lanelet56)]=}""")
+    print(
+        f"""{[ll2.id for ll2 in query.getAllNeighbors(routing_graph, query.roadLanelets(lanelets), search_point)]=}"""
+    )
+    print(f"""{[ll2.id for ll2 in query.getAllNeighborsLeft(routing_graph, lanelet56)]=}""")
+    print(f"""{[ll2.id for ll2 in query.getAllNeighborsRight(routing_graph, lanelet56)]=}""")
+    search_pose = Pose()
+    search_pose.position = search_point
+    print(
+        f"""{(query.getClosestLanelet(lanelets, search_pose).id if query.getClosestLanelet(lanelets, search_pose) else None)=}"""
+    )
+    print(f"""{[ll2.id for ll2 in query.getCurrentLanelets(lanelets, search_point)]=}""")
+    print(
+        f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getSucceedingLaneletSequences(routing_graph, lanelet108, 100.0)]=}"""
+    )
+    print(
+        f"""{[[ll2.id for ll2 in ll2s] for ll2s in query.getPrecedingLaneletSequences(routing_graph, lanelet108, 100.0)]=}"""
+    )
+
+
+if __name__ == "__main__":
+    proj = test_projection()
+    # https://docs.google.com/uc?export=download&id=1499_nsbUbIeturZaDj7jhUownh5fvXHd.
+    # See https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/
+    lanelet_map = test_io("<path to lanelet2_map.osm>", proj)
+
+    traffic_rules = lanelet2.traffic_rules.create(
+        lanelet2.traffic_rules.Locations.Germany,
+        lanelet2.traffic_rules.Participants.Vehicle,
+    )
+    routing_graph = lanelet2.routing.RoutingGraph(lanelet_map, traffic_rules)
+
+    test_utility_utilities(lanelet_map, routing_graph)
+    test_utility_query(lanelet_map, routing_graph)
diff --git a/lanelet2_extension_python/lanelet2_extension_python/__init__.py b/lanelet2_extension_python/lanelet2_extension_python/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/lanelet2_extension_python/lanelet2_extension_python/impl/__init__.py b/lanelet2_extension_python/lanelet2_extension_python/impl/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/lanelet2_extension_python/lanelet2_extension_python/projection/__init__.py b/lanelet2_extension_python/lanelet2_extension_python/projection/__init__.py
new file mode 100644
index 0000000..b5e7067
--- /dev/null
+++ b/lanelet2_extension_python/lanelet2_extension_python/projection/__init__.py
@@ -0,0 +1,3 @@
+import lanelet2_extension_python._lanelet2_extension_python_boost_python_projection as _projection_cpp
+
+MGRSProjector = _projection_cpp.MGRSProjector
diff --git a/lanelet2_extension_python/lanelet2_extension_python/regulatory_elements/__init__.py b/lanelet2_extension_python/lanelet2_extension_python/regulatory_elements/__init__.py
new file mode 100644
index 0000000..010ecc9
--- /dev/null
+++ b/lanelet2_extension_python/lanelet2_extension_python/regulatory_elements/__init__.py
@@ -0,0 +1,10 @@
+import lanelet2_extension_python._lanelet2_extension_python_boost_python_regulatory_elements as _regulatory_elements_cpp
+
+AutowareTrafficLight = _regulatory_elements_cpp.AutowareTrafficLight
+Crosswalk = _regulatory_elements_cpp.Crosswalk
+DetectionArea = _regulatory_elements_cpp.DetectionArea
+NoParkingArea = _regulatory_elements_cpp.NoParkingArea
+NoStoppingArea = _regulatory_elements_cpp.NoStoppingArea
+RoadMarking = _regulatory_elements_cpp.RoadMarking
+SpeedBump = _regulatory_elements_cpp.SpeedBump
+VirtualTrafficLight = _regulatory_elements_cpp.VirtualTrafficLight
diff --git a/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py b/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py
new file mode 100644
index 0000000..5ba63f1
--- /dev/null
+++ b/lanelet2_extension_python/lanelet2_extension_python/utility/__init__.py
@@ -0,0 +1 @@
+import lanelet2_extension_python.regulatory_elements  # noqa
diff --git a/lanelet2_extension_python/lanelet2_extension_python/utility/query.py b/lanelet2_extension_python/lanelet2_extension_python/utility/query.py
new file mode 100644
index 0000000..7ac7206
--- /dev/null
+++ b/lanelet2_extension_python/lanelet2_extension_python/utility/query.py
@@ -0,0 +1,95 @@
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Pose
+import lanelet2.core
+import lanelet2_extension_python._lanelet2_extension_python_boost_python_utility as _utility_cpp
+
+# from rclpy.serialization import deserialize_message
+from rclpy.serialization import serialize_message
+
+laneletLayer = _utility_cpp.laneletLayer
+subtypeLanelets = _utility_cpp.subtypeLanelets
+crosswalkLanelets = _utility_cpp.crosswalkLanelets
+walkwayLanelets = _utility_cpp.walkwayLanelets
+roadLanelets = _utility_cpp.roadLanelets
+shoulderLanelets = _utility_cpp.shoulderLanelets
+trafficLights = _utility_cpp.trafficLights
+autowareTrafficLights = _utility_cpp.autowareTrafficLights
+detectionAreas = _utility_cpp.detectionAreas
+noStoppingAreas = _utility_cpp.noStoppingAreas
+noParkingAreas = _utility_cpp.noParkingAreas
+speedBumps = _utility_cpp.speedBumps
+crosswalks = _utility_cpp.crosswalks
+curbstones = _utility_cpp.curbstones
+getAllPolygonsByType = _utility_cpp.getAllPolygonsByType
+getAllObstaclePolygons = _utility_cpp.getAllObstaclePolygons
+getAllParkingLots = _utility_cpp.getAllParkingLots
+getAllPartitions = _utility_cpp.getAllPartitions
+getAllFences = _utility_cpp.getAllFences
+getAllPedestrianPolygonMarkings = _utility_cpp.getAllPedestrianPolygonMarkings
+getAllPedestrianLineMarkings = _utility_cpp.getAllPedestrianLineMarkings
+getAllParkingSpaces = _utility_cpp.getAllParkingSpaces
+getLinkedParkingSpaces = _utility_cpp.getLinkedParkingSpaces
+getLinkedLanelet = _utility_cpp.getLinkedLanelet
+getLinkedLanelets = _utility_cpp.getLinkedLanelets
+getLinkedParkingLot = _utility_cpp.getLinkedParkingLot
+stopLinesLanelets = _utility_cpp.stopLinesLanelets
+stopLinesLanelet = _utility_cpp.stopLinesLanelet
+stopSignStopLines = _utility_cpp.stopSignStopLines
+
+
+def getLaneletsWithinRange(lanelets, point, rng):
+    if isinstance(point, Point):
+        point_byte = serialize_message(point)
+        return _utility_cpp.getLaneletsWithinRange_point(lanelets, point_byte, rng)
+    if isinstance(point, lanelet2.core.BasicPoint2d):
+        return _utility_cpp.getLaneletsWithinRange(lanelets, point, rng)
+    raise TypeError("argument point is neither BasicPoint2d nor Point")
+
+
+def getLaneChangeableNeighbors(*args):
+    if len(args) == 2 and isinstance(args[1], lanelet2.core.Lanelet):
+        return _utility_cpp.getLaneChangeableNeighbors(args[0], args[1])
+    if len(args) == 3 and isinstance(args[2], Point):
+        point_byte = serialize_message(args[2])
+        return _utility_cpp.getLaneChangeableNeighbors_point(args[0], args[1], point_byte)
+    raise TypeError("argument number does not match or 3rd argument is not Point type")
+
+
+def getAllNeighbors(*args):
+    if len(args) == 2 and isinstance(args[1], lanelet2.core.Lanelet):
+        return _utility_cpp.getAllNeighbors(args[0], args[1])
+    if len(args) == 3 and isinstance(args[2], Point):
+        point_byte = serialize_message(args[2])
+        return _utility_cpp.getAllNeighbors_point(args[0], args[1], point_byte)
+
+
+getAllNeighborsLeft = _utility_cpp.getAllNeighborsLeft
+getAllNeighborsRight = _utility_cpp.getAllNeighborsRight
+
+
+def getClosestLanelet(lanelets, pose: Pose):
+    pose_byte = serialize_message(pose)
+    return _utility_cpp.getClosestLanelet(lanelets, pose_byte)
+
+
+def getClosestLaneletWithConstrains(
+    lanelets, pose: Pose, closest_lanelet, dist_threshold, yaw_threshold
+):
+    pose_byte = serialize_message(pose)
+    return _utility_cpp.getClosestLaneletWithConstrains(
+        lanelets, pose_byte, closest_lanelet, dist_threshold, yaw_threshold
+    )
+
+
+def getCurrentLanelets(lanelets, point: Point):
+    if isinstance(point, Point):
+        point_byte = serialize_message(point)
+        return _utility_cpp.getCurrentLanelets_point(lanelets, point_byte)
+    if isinstance(point, Pose):
+        pose_byte = serialize_message(point)
+        return _utility_cpp.getCurrentLanelets_pose(lanelets, pose_byte)
+    raise TypeError("argument number does not match or 2nd argument is not Point/Pose type")
+
+
+getSucceedingLaneletSequences = _utility_cpp.getSucceedingLaneletSequences
+getPrecedingLaneletSequences = _utility_cpp.getPrecedingLaneletSequences
diff --git a/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py b/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py
new file mode 100644
index 0000000..27acf99
--- /dev/null
+++ b/lanelet2_extension_python/lanelet2_extension_python/utility/utilities.py
@@ -0,0 +1,75 @@
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Quaternion
+import lanelet2
+import lanelet2_extension_python._lanelet2_extension_python_boost_python_utility as _utility_cpp
+from rclpy.serialization import serialize_message
+
+combineLaneletsShape = _utility_cpp.combineLaneletsShape
+generateFineCenterline = _utility_cpp.generateFineCenterline
+getCenterlineWithOffset = _utility_cpp.getCenterlineWithOffset
+getRightBoundWithOffset = _utility_cpp.getRightBoundWithOffset
+getLeftBoundWithOffset = _utility_cpp.getLeftBoundWithOffset
+getExpandedLanelet = _utility_cpp.getExpandedLanelet
+getExpandedLanelets = _utility_cpp.getExpandedLanelets
+overwriteLaneletsCenterline = _utility_cpp.overwriteLaneletsCenterline
+getConflictingLanelets = _utility_cpp.getConflictingLanelets
+lineStringWithWidthToPolygon = _utility_cpp.lineStringWithWidthToPolygon
+lineStringToPolygon = _utility_cpp.lineStringToPolygon
+
+
+def getLaneletLength2d(*args):
+    if len(args) == 1 and isinstance(args[0], lanelet2.core.Lanelet):
+        return _utility_cpp.getLaneletLength2d(args[0])
+    if len(args) == 1 and isinstance(args[0], list):
+        return _utility_cpp.getLaneletLength2d(args[0])
+    raise TypeError(
+        "argument number does not match or 1st argument is not Lanelet or [Lanelet] type"
+    )
+
+
+def getLaneletLength3d(*args):
+    if len(args) == 1 and isinstance(args[0], lanelet2.core.Lanelet):
+        return _utility_cpp.getLaneletLength3d(args[0])
+    if len(args) == 1 and isinstance(args[0], list):
+        return _utility_cpp.getLaneletLength3d(args[0])
+    raise TypeError(
+        "argument number does not match or 1st argument is not Lanelet or [Lanelet] type"
+    )
+
+
+def getArcCoordinates(lanelet_sequence, pose: Pose):
+    pose_byte = serialize_message(pose)
+    return _utility_cpp.getArcCoordinates(lanelet_sequence, pose_byte)
+
+
+getClosestSegment = _utility_cpp.getClosestSegment
+getPolygonFromArcLength = _utility_cpp.getPolygonFromArcLength
+
+
+def getLaneletAngle(lanelet, point: Point):
+    point_byte = serialize_message(point)
+    return _utility_cpp.getLaneletAngle(lanelet, point_byte)
+
+
+def isInLanelet(pose: Pose, lanelet, radius=0.0):
+    pose_byte = serialize_message(pose)
+    return _utility_cpp.isInLanelet(pose_byte, lanelet, radius)
+
+
+def getClosestCenterPose(lanelet, point: Point):
+    point_byte = serialize_message(point)
+    pose_array = _utility_cpp.getClosestCenterPose(lanelet, point_byte)
+    pos = Point(x=pose_array[0], y=pose_array[1], z=pose_array[2])
+    quat = Quaternion(x=pose_array[3], y=pose_array[4], z=pose_array[5], w=pose_array[6])
+    return Pose(position=pos, orientation=quat)
+
+
+def getLateralDistanceToCenterline(lanelet, pose: Pose):
+    pose_byte = serialize_message(pose)
+    return _utility_cpp.getLateralDistanceToCenterline(lanelet, pose_byte)
+
+
+def getLateralDistanceToClosestLanelet(lanelet_sequence, pose: Pose):
+    pose_byte = serialize_message(pose)
+    return _utility_cpp.getLateralDistanceToClosestLanelet(lanelet_sequence, pose_byte)
diff --git a/lanelet2_extension_python/package.xml b/lanelet2_extension_python/package.xml
new file mode 100644
index 0000000..964ea43
--- /dev/null
+++ b/lanelet2_extension_python/package.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lanelet2_extension_python</name>
+  <version>0.1.0</version>
+  <description>The lanelet2_extension_python package contains Python bindings for lanelet2_extension package</description>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
+  <maintainer email="yutaka.kondo@tier4.jp">Yutaka Kondo</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>python_cmake_module</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_core</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>lanelet2_io</depend>
+  <depend>lanelet2_projection</depend>
+  <depend>lanelet2_python</depend>
+  <depend>lanelet2_routing</depend>
+  <depend>lanelet2_traffic_rules</depend>
+  <depend>lanelet2_validation</depend>
+  <depend>libboost-python-dev</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/lanelet2_extension_python/src/projection.cpp b/lanelet2_extension_python/src/projection.cpp
new file mode 100644
index 0000000..1f70a21
--- /dev/null
+++ b/lanelet2_extension_python/src/projection.cpp
@@ -0,0 +1,42 @@
+// Copyright 2023 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Mamoru Sobue
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <lanelet2_extension/projection/transverse_mercator_projector.hpp>
+
+#include <boost/python.hpp>
+
+#include <lanelet2_io/Projection.h>
+
+#include <memory>
+
+BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_projection)
+{
+  namespace bp = boost::python;
+
+  bp::class_<
+    lanelet::projection::MGRSProjector, std::shared_ptr<lanelet::projection::MGRSProjector>,
+    bp::bases<lanelet::Projector>>("MGRSProjector", bp::init<lanelet::Origin>("origin"));
+  bp::class_<
+    lanelet::projection::TransverseMercatorProjector,
+    std::shared_ptr<lanelet::projection::TransverseMercatorProjector>,
+    bp::bases<lanelet::Projector>>(
+    "TransverseMercatorProjector", bp::init<lanelet::Origin>("origin"));
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension_python/src/regulatory_elements.cpp b/lanelet2_extension_python/src/regulatory_elements.cpp
new file mode 100644
index 0000000..acddb81
--- /dev/null
+++ b/lanelet2_extension_python/src/regulatory_elements.cpp
@@ -0,0 +1,307 @@
+// Copyright 2023 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Mamoru Sobue
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
+#include <lanelet2_extension/regulatory_elements/crosswalk.hpp>
+#include <lanelet2_extension/regulatory_elements/detection_area.hpp>
+#include <lanelet2_extension/regulatory_elements/no_parking_area.hpp>
+#include <lanelet2_extension/regulatory_elements/no_stopping_area.hpp>
+#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
+#include <lanelet2_extension/regulatory_elements/speed_bump.hpp>
+#include <lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp>
+
+#include <boost/python.hpp>
+
+namespace bp = boost::python;
+
+// from lanelet2_python/src/core.cpp
+
+void format_helper([[maybe_unused]] std::ostream & os)
+{
+}
+
+template <typename... Args>
+void format_helper(std::ostream & os, const std::string & s, const Args &... Args_)
+{
+  if (!s.empty()) {
+    os << ", ";
+  }
+  os << s;
+  format_helper(os, Args_...);
+}
+
+template <typename T, typename... Args>
+void format_helper(std::ostream & os, const T & next, const Args &... Args_)
+{
+  os << ", ";
+  os << next;
+  format_helper(os, Args_...);
+}
+
+template <typename T, typename... Args>
+void format(std::ostream & os, const T & first, const Args &... Args_)
+{
+  os << first;
+  format_helper(os, Args_...);
+}
+
+template <typename... Args>
+std::string make_repr(const char * name, const Args &... Args_)
+{
+  std::ostringstream os;
+  os << name << '(';
+  format(os, Args_...);
+  os << ')';
+  return os.str();
+}
+
+std::string repr(const bp::object & o)
+{
+  bp::object repr = bp::import("builtins").attr("repr");
+  return bp::call<std::string>(repr.ptr(), o);
+}
+
+std::string repr(const lanelet::AttributeMap & a)
+{
+  if (a.empty()) {
+    return {};
+  }
+  return repr(bp::object(a));
+}
+
+std::string repr(const lanelet::RegulatoryElementConstPtrs & regulatory_elements)
+{
+  if (regulatory_elements.empty()) {
+    return {};
+  }
+  return repr(bp::list(regulatory_elements));
+}
+
+BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_regulatory_elements)
+{
+  // autoware_traffic_light
+  bp::class_<
+    lanelet::autoware::AutowareTrafficLight, boost::noncopyable,
+    std::shared_ptr<lanelet::autoware::AutowareTrafficLight>, bp::bases<lanelet::TrafficLight>>(
+    "AutowareTrafficLight", "autoware traffic light regulatory element", bp::no_init)
+    .def(
+      "__init__", bp::make_constructor(
+                    &lanelet::autoware::AutowareTrafficLight::make, bp::default_call_policies(),
+                    (bp::arg("Id"), bp::arg("attributes"), bp::arg("trafficLights"),
+                     bp::arg("stopline") = lanelet::Optional<lanelet::LineString3d>(),
+                     bp::arg("lightBulbs") = lanelet::LineString3d({}))))
+    .def(
+      "lightBulbs",
+      +[](lanelet::autoware::AutowareTrafficLight & self) { return self.lightBulbs(); })
+    .def("addLightBulbs", &lanelet::autoware::AutowareTrafficLight::addLightBulbs)
+    .def("removeLightBulbs", &lanelet::autoware::AutowareTrafficLight::removeLightBulbs)
+    .def(
+      "__repr__", +[](lanelet::autoware::AutowareTrafficLight & r) {
+        return make_repr(
+          "AutowareTrafficLight", r.id(), repr(bp::dict(r.constData()->parameters)),
+          repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::AutowareTrafficLight>, lanelet::RegulatoryElementPtr>();
+
+  // crosswalk
+  bp::class_<
+    lanelet::autoware::Crosswalk, boost::noncopyable, std::shared_ptr<lanelet::autoware::Crosswalk>,
+    bp::bases<lanelet::RegulatoryElement>>(
+    "Crosswalk", "autoware crosswalk regulatory element", bp::no_init)
+    .def(
+      "__init__", bp::make_constructor(
+                    &lanelet::autoware::Crosswalk::make, bp::default_call_policies(),
+                    (bp::arg("Id"), bp::arg("attributes"), bp::arg("crosswalk_lanelet"),
+                     bp::arg("crosswalk_area"), bp::arg("stop_line"))))
+    .def(
+      "crosswalkAreas", +[](lanelet::autoware::Crosswalk & self) { return self.crosswalkAreas(); })
+    .def(
+      "stopLines", +[](lanelet::autoware::Crosswalk & self) { return self.stopLines(); })
+    .def(
+      "crosswalkLanelet",
+      +[](lanelet::autoware::Crosswalk & self) { return self.crosswalkLanelet(); })
+    .def("addCrosswalkArea", &lanelet::autoware::Crosswalk::addCrosswalkArea)
+    .def("removeCrosswalkArea", &lanelet::autoware::Crosswalk::removeCrosswalkArea)
+    .def(
+      "__repr__", +[](lanelet::autoware::Crosswalk & r) {
+        return make_repr(
+          "Crosswalk", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::Crosswalk>, lanelet::RegulatoryElementPtr>();
+
+  // detection_area
+  bp::class_<
+    lanelet::autoware::DetectionArea, boost::noncopyable,
+    std::shared_ptr<lanelet::autoware::DetectionArea>, bp::bases<lanelet::RegulatoryElement>>(
+    "DetectionArea", "detection_area regulatory element", bp::no_init)
+    .def(
+      "__init__",
+      bp::make_constructor(
+        &lanelet::autoware::DetectionArea::make, bp::default_call_policies(),
+        (bp::arg("Id"), bp::arg("attributes"), bp::arg("detectionAreas"), bp::arg("stopLine"))))
+    .def(
+      "detectionAreas",
+      +[](lanelet::autoware::DetectionArea & self) { return self.detectionAreas(); })
+    .def("addDetectionArea", &lanelet::autoware::DetectionArea::addDetectionArea)
+    .def("removeDetectionArea", &lanelet::autoware::DetectionArea::removeDetectionArea)
+    .def(
+      "stopLine", +[](lanelet::autoware::DetectionArea & self) { return self.stopLine(); })
+    .def("setStopLine", &lanelet::autoware::DetectionArea::setStopLine)
+    .def("removeStopLine", &lanelet::autoware::DetectionArea::removeStopLine)
+    .def(
+      "__repr__", +[](lanelet::autoware::DetectionArea & r) {
+        return make_repr(
+          "DetectionArea", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::DetectionArea>, lanelet::RegulatoryElementPtr>();
+
+  // no_parking_area
+  bp::class_<
+    lanelet::autoware::NoParkingArea, boost::noncopyable,
+    std::shared_ptr<lanelet::autoware::NoParkingArea>, bp::bases<lanelet::RegulatoryElement>>(
+    "NoParkingArea", "no_parking_area regulatory element", bp::no_init)
+    .def(
+      "__init__", bp::make_constructor(
+                    &lanelet::autoware::NoParkingArea::make, bp::default_call_policies(),
+                    (bp::arg("Id"), bp::arg("attributes"), bp::arg("no_parking_areas"))))
+    .def(
+      "noParkingAreas",
+      +[](lanelet::autoware::NoParkingArea & self) { return self.noParkingAreas(); })
+    .def("addNoParkingArea", &lanelet::autoware::NoParkingArea::addNoParkingArea)
+    .def("removeNoParkingArea", &lanelet::autoware::NoParkingArea::removeNoParkingArea)
+    .def(
+      "__repr__", +[](lanelet::autoware::NoParkingArea & r) {
+        return make_repr(
+          "NoParkingArea", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::NoParkingArea>, lanelet::RegulatoryElementPtr>();
+
+  // no_stopping_area
+  bp::class_<
+    lanelet::autoware::NoStoppingArea, boost::noncopyable,
+    std::shared_ptr<lanelet::autoware::NoStoppingArea>, bp::bases<lanelet::RegulatoryElement>>(
+    "NoStoppingArea", "no_stopping_area regulatory element", bp::no_init)
+    .def(
+      "__init__", bp::make_constructor(
+                    &lanelet::autoware::NoStoppingArea::make, bp::default_call_policies(),
+                    (bp::arg("Id"), bp::arg("attributes"), bp::arg("no_stopping_areas"),
+                     bp::arg("stopLine") = lanelet::Optional<lanelet::LineString3d>())))
+    .def(
+      "noStoppingAreas",
+      +[](lanelet::autoware::NoStoppingArea & self) { return self.noStoppingAreas(); })
+    .def("addNoStoppingArea", &lanelet::autoware::NoStoppingArea::addNoStoppingArea)
+    .def("removeNoStoppingArea", &lanelet::autoware::NoStoppingArea::removeNoStoppingArea)
+    .def(
+      "stopLine", +[](lanelet::autoware::NoStoppingArea & self) { return self.stopLine(); })
+    .def("setStopLine", &lanelet::autoware::NoStoppingArea::setStopLine)
+    .def("removeStopLine", &lanelet::autoware::NoStoppingArea::removeStopLine)
+    .def(
+      "__repr__", +[](lanelet::autoware::NoParkingArea & r) {
+        return make_repr(
+          "NoParkingArea", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::NoStoppingArea>, lanelet::RegulatoryElementPtr>();
+
+  // road_marking
+  bp::class_<
+    lanelet::autoware::RoadMarking, boost::noncopyable,
+    std::shared_ptr<lanelet::autoware::RoadMarking>, bp::bases<lanelet::RegulatoryElement>>(
+    "RoadMarking", "road_marking regulatory element", bp::no_init)
+    .def(
+      "__init__", bp::make_constructor(
+                    &lanelet::autoware::RoadMarking::make, bp::default_call_policies(),
+                    (bp::arg("Id"), bp::arg("attributes"), bp::arg("road_marking"))))
+    .def(
+      "roadMarking", +[](lanelet::autoware::RoadMarking & self) { return self.roadMarking(); })
+    .def("setRoadMarking", &lanelet::autoware::RoadMarking::setRoadMarking)
+    .def("removeRoadMarking", &lanelet::autoware::RoadMarking::removeRoadMarking)
+    .def(
+      "__repr__", +[](lanelet::autoware::RoadMarking & r) {
+        return make_repr(
+          "RoadMarking", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::RoadMarking>, lanelet::RegulatoryElementPtr>();
+
+  // speed_bump
+  bp::class_<
+    lanelet::autoware::SpeedBump, boost::noncopyable, std::shared_ptr<lanelet::autoware::SpeedBump>,
+    bp::bases<lanelet::RegulatoryElement>>(
+    "SpeedBump", "speed_bump regulatory element", bp::no_init)
+    .def(
+      "__init__", bp::make_constructor(
+                    &lanelet::autoware::SpeedBump::make, bp::default_call_policies(),
+                    (bp::arg("Id"), bp::arg("attributes"), bp::arg("speed_bump"))))
+    .def(
+      "speedBump", +[](lanelet::autoware::SpeedBump & self) { return self.speedBump(); })
+    .def("addSpeedBump", &lanelet::autoware::SpeedBump::addSpeedBump)
+    .def("removeSpeedBump", &lanelet::autoware::SpeedBump::removeSpeedBump)
+    .def(
+      "__repr__", +[](lanelet::autoware::SpeedBump & r) {
+        return make_repr(
+          "SpeedBump", r.id(), repr(bp::dict(r.constData()->parameters)), repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::SpeedBump>, lanelet::RegulatoryElementPtr>();
+
+  // virtual_traffic_light
+  bp::class_<
+    lanelet::autoware::VirtualTrafficLight, boost::noncopyable,
+    std::shared_ptr<lanelet::autoware::VirtualTrafficLight>, bp::bases<lanelet::RegulatoryElement>>(
+    "VirtualTrafficLight", "virtual_traffic_light regulatory element", bp::no_init)
+    .def(
+      "__init__", bp::make_constructor(
+                    &lanelet::autoware::VirtualTrafficLight::make, bp::default_call_policies(),
+                    (bp::arg("Id"), bp::arg("attributes"), bp::arg("virtual_traffic_light"))))
+    .def(
+      "getVirtualTrafficLight",
+      +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getVirtualTrafficLight(); })
+    .def(
+      "getStopLine",
+      +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getStopLine(); })
+    .def(
+      "getStartLine",
+      +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getStartLine(); })
+    .def(
+      "getEndLines",
+      +[](lanelet::autoware::VirtualTrafficLight & self) { return self.getEndLines(); })
+    .def(
+      "__repr__", +[](lanelet::autoware::VirtualTrafficLight & r) {
+        return make_repr(
+          "VirtualTrafficLight", r.id(), repr(bp::dict(r.constData()->parameters)),
+          repr(r.attributes()));
+      });
+  bp::implicitly_convertible<
+    std::shared_ptr<lanelet::autoware::VirtualTrafficLight>, lanelet::RegulatoryElementPtr>();
+
+  bp::register_ptr_to_python<lanelet::TrafficSignConstPtr>();
+  bp::register_ptr_to_python<lanelet::TrafficLightConstPtr>();
+  bp::register_ptr_to_python<lanelet::AutowareTrafficLightConstPtr>();
+  bp::register_ptr_to_python<lanelet::DetectionAreaConstPtr>();
+  bp::register_ptr_to_python<lanelet::NoParkingAreaConstPtr>();
+  bp::register_ptr_to_python<lanelet::NoStoppingAreaConstPtr>();
+  bp::register_ptr_to_python<lanelet::SpeedBumpConstPtr>();
+  bp::register_ptr_to_python<lanelet::CrosswalkConstPtr>();
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/lanelet2_extension_python/src/utility.cpp b/lanelet2_extension_python/src/utility.cpp
new file mode 100644
index 0000000..fdd612f
--- /dev/null
+++ b/lanelet2_extension_python/src/utility.cpp
@@ -0,0 +1,564 @@
+// Copyright 2023 Autoware Foundation. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Mamoru Sobue
+
+// NOLINTBEGIN(readability-identifier-naming)
+
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/serialization.hpp>
+
+#include <boost/python.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+#include <lanelet2_core/geometry/Polygon.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_python/internal/converter.h>
+
+#include <iostream>
+#include <string>
+
+namespace bp = boost::python;
+
+namespace
+{
+
+/*
+ * utilities.cpp
+ */
+lanelet::Optional<lanelet::ConstPolygon3d> lineStringWithWidthToPolygon(
+  const lanelet::ConstLineString3d & linestring)
+{
+  lanelet::ConstPolygon3d poly{};
+  if (lanelet::utils::lineStringWithWidthToPolygon(linestring, &poly)) {
+    return poly;
+  } else {
+    return {};
+  }
+}
+
+lanelet::Optional<lanelet::ConstPolygon3d> lineStringToPolygon(
+  const lanelet::ConstLineString3d & linestring)
+{
+  lanelet::ConstPolygon3d poly{};
+  if (lanelet::utils::lineStringToPolygon(linestring, &poly)) {
+    return poly;
+  } else {
+    return {};
+  }
+}
+
+lanelet::ArcCoordinates getArcCoordinates(
+  const lanelet::ConstLanelets & lanelet_sequence, const std::string & pose_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + pose_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
+  for (size_t i = 0; i < pose_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+  }
+  geometry_msgs::msg::Pose pose;
+  static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
+  serializer.deserialize_message(&serialized_msg, &pose);
+  return lanelet::utils::getArcCoordinates(lanelet_sequence, pose);
+}
+
+double getLaneletAngle(const lanelet::ConstLanelet & lanelet, const std::string & point_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + point_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
+  for (size_t i = 0; i < point_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+  }
+  geometry_msgs::msg::Point point;
+  static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
+  serializer.deserialize_message(&serialized_msg, &point);
+  return lanelet::utils::getLaneletAngle(lanelet, point);
+}
+
+bool isInLanelet(
+  const std::string & pose_byte, const lanelet::ConstLanelet & lanelet, const double radius = 0.0)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + pose_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
+  for (size_t i = 0; i < pose_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+  }
+  geometry_msgs::msg::Pose pose;
+  static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
+  serializer.deserialize_message(&serialized_msg, &pose);
+  return lanelet::utils::isInLanelet(pose, lanelet, radius);
+}
+
+std::vector<double> getClosestCenterPose(
+  const lanelet::ConstLanelet & lanelet, const std::string & search_point_byte)
+{
+  rclcpp::SerializedMessage serialized_point_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_point_msg.reserve(message_header_length + search_point_byte.size());
+  serialized_point_msg.get_rcl_serialized_message().buffer_length = search_point_byte.size();
+  for (size_t i = 0; i < search_point_byte.size(); ++i) {
+    serialized_point_msg.get_rcl_serialized_message().buffer[i] = search_point_byte[i];
+  }
+  geometry_msgs::msg::Point search_point;
+  static rclcpp::Serialization<geometry_msgs::msg::Point> serializer_point;
+  serializer_point.deserialize_message(&serialized_point_msg, &search_point);
+  const geometry_msgs::msg::Pose pose = lanelet::utils::getClosestCenterPose(lanelet, search_point);
+  // NOTE: it was difficult to return the deserialized pose_byte and serialize the pose_byte on
+  // python-side. So this function returns [*position, *quaternion] as double array
+  const auto & xyz = pose.position;
+  const auto & quat = pose.orientation;
+  return std::vector<double>({xyz.x, xyz.y, xyz.z, quat.x, quat.y, quat.z, quat.w});
+}
+
+double getLateralDistanceToCenterline(
+  const lanelet::ConstLanelet & lanelet, const std::string & pose_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + pose_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
+  for (size_t i = 0; i < pose_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+  }
+  geometry_msgs::msg::Pose pose;
+  static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
+  serializer.deserialize_message(&serialized_msg, &pose);
+  return lanelet::utils::getLateralDistanceToCenterline(lanelet, pose);
+}
+
+double getLateralDistanceToClosestLanelet(
+  const lanelet::ConstLanelets & lanelet_sequence, const std::string & pose_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + pose_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
+  for (size_t i = 0; i < pose_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+  }
+  geometry_msgs::msg::Pose pose;
+  static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
+  serializer.deserialize_message(&serialized_msg, &pose);
+  return lanelet::utils::getLateralDistanceToClosestLanelet(lanelet_sequence, pose);
+}
+
+/*
+ * query.cpp
+ */
+
+lanelet::ConstLanelets subtypeLanelets(
+  const lanelet::ConstLanelets & lls, const std::string & subtype)
+{
+  return lanelet::utils::query::subtypeLanelets(lls, subtype.c_str());
+}
+
+lanelet::Optional<lanelet::ConstLanelet> getLinkedLanelet(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstLanelets & all_road_lanelets,
+  const lanelet::ConstPolygons3d & all_parking_lots)
+{
+  lanelet::ConstLanelet linked_lanelet;
+  if (lanelet::utils::query::getLinkedLanelet(
+        parking_space, all_road_lanelets, all_parking_lots, &linked_lanelet)) {
+    return linked_lanelet;
+  } else {
+    return {};
+  }
+}
+
+lanelet::Optional<lanelet::ConstLanelet> getLinkedLanelet(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLanelet linked_lanelet;
+  if (lanelet::utils::query::getLinkedLanelet(parking_space, lanelet_map_ptr, &linked_lanelet)) {
+    return linked_lanelet;
+  } else {
+    return {};
+  }
+}
+
+lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
+  const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots)
+{
+  lanelet::ConstPolygon3d linked_parking_lot;
+  if (lanelet::utils::query::getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) {
+    return linked_parking_lot;
+  } else {
+    return {};
+  }
+}
+
+lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
+  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots)
+{
+  lanelet::ConstPolygon3d linked_parking_lot;
+  if (lanelet::utils::query::getLinkedParkingLot(
+        current_position, all_parking_lots, &linked_parking_lot)) {
+    return linked_parking_lot;
+  } else {
+    return {};
+  }
+}
+
+lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
+  const lanelet::ConstLineString3d & parking_space,
+  const lanelet::ConstPolygons3d & all_parking_lots)
+{
+  lanelet::ConstPolygon3d linked_parking_lot;
+  if (lanelet::utils::query::getLinkedParkingLot(
+        parking_space, all_parking_lots, &linked_parking_lot)) {
+    return linked_parking_lot;
+  } else {
+    return {};
+  }
+}
+
+lanelet::ConstLanelets getLaneletsWithinRange_point(
+  const lanelet::ConstLanelets & lanelets, const std::string & point_byte, const double range)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + point_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
+  for (size_t i = 0; i < point_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+  }
+  geometry_msgs::msg::Point point;
+  static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
+  serializer.deserialize_message(&serialized_msg, &point);
+  return lanelet::utils::query::getLaneletsWithinRange(lanelets, point, range);
+}
+
+lanelet::ConstLanelets getLaneChangeableNeighbors_point(
+  const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelets & road_lanelets,
+  const std::string & point_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + point_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
+  for (size_t i = 0; i < point_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+  }
+  geometry_msgs::msg::Point point;
+  static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
+  serializer.deserialize_message(&serialized_msg, &point);
+  return lanelet::utils::query::getLaneChangeableNeighbors(graph, road_lanelets, point);
+}
+
+lanelet::ConstLanelets getAllNeighbors_point(
+  const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelets & road_lanelets,
+  const std::string & point_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + point_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
+  for (size_t i = 0; i < point_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+  }
+  geometry_msgs::msg::Point point;
+  static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
+  serializer.deserialize_message(&serialized_msg, &point);
+  return lanelet::utils::query::getAllNeighbors(graph, road_lanelets, point);
+}
+
+lanelet::Optional<lanelet::ConstLanelet> getClosestLanelet(
+  const lanelet::ConstLanelets & lanelets, const std::string & pose_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + pose_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
+  for (size_t i = 0; i < pose_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+  }
+  geometry_msgs::msg::Pose pose;
+  static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
+  serializer.deserialize_message(&serialized_msg, &pose);
+  lanelet::ConstLanelet closest_lanelet{};
+  if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) {
+    return closest_lanelet;
+  } else {
+    return {};
+  }
+}
+
+lanelet::Optional<lanelet::ConstLanelet> getClosestLaneletWithConstrains(
+  const lanelet::ConstLanelets & lanelets, const std::string & pose_byte,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max())
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + pose_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
+  for (size_t i = 0; i < pose_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+  }
+  geometry_msgs::msg::Pose pose;
+  static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
+  serializer.deserialize_message(&serialized_msg, &pose);
+  lanelet::ConstLanelet closest_lanelet{};
+  if (lanelet::utils::query::getClosestLaneletWithConstrains(
+        lanelets, pose, &closest_lanelet, dist_threshold, yaw_threshold)) {
+    return closest_lanelet;
+  } else {
+    return {};
+  }
+}
+
+lanelet::ConstLanelets getCurrentLanelets_point(
+  const lanelet::ConstLanelets & lanelets, const std::string & point_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + point_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
+  for (size_t i = 0; i < point_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+  }
+  geometry_msgs::msg::Point point;
+  static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
+  serializer.deserialize_message(&serialized_msg, &point);
+  lanelet::ConstLanelets current_lanelets{};
+  lanelet::utils::query::getCurrentLanelets(lanelets, point, &current_lanelets);
+  return current_lanelets;
+}
+
+lanelet::ConstLanelets getCurrentLanelets_pose(
+  const lanelet::ConstLanelets & lanelets, const std::string & pose_byte)
+{
+  rclcpp::SerializedMessage serialized_msg;
+  static constexpr size_t message_header_length = 8u;
+  serialized_msg.reserve(message_header_length + pose_byte.size());
+  serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
+  for (size_t i = 0; i < pose_byte.size(); ++i) {
+    serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+  }
+  geometry_msgs::msg::Pose pose;
+  static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
+  serializer.deserialize_message(&serialized_msg, &pose);
+  lanelet::ConstLanelets current_lanelets{};
+  lanelet::utils::query::getCurrentLanelets(lanelets, pose, &current_lanelets);
+  return current_lanelets;
+}
+
+}  // namespace
+
+// for handling functions with default arguments
+/// utilities.cpp
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  generateFineCenterline_overload, lanelet::utils::generateFineCenterline, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  getCenterlineWithOffset_overload, lanelet::utils::getCenterlineWithOffset, 2, 3)
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  getRightBoundWithOffset_overload, lanelet::utils::getRightBoundWithOffset, 2, 3)
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  getLeftBoundWithOffset_overload, lanelet::utils::getLeftBoundWithOffset, 2, 3)
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  overwriteLaneletsCenterline_overload, lanelet::utils::overwriteLaneletsCenterline, 1, 3)
+BOOST_PYTHON_FUNCTION_OVERLOADS(isInLanelet_overload, ::isInLanelet, 2, 3)
+
+/// query.cpp
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  stopSignStopLines_overload, lanelet::utils::query::stopSignStopLines, 1, 2)
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 2, 4)
+BOOST_PYTHON_FUNCTION_OVERLOADS(
+  getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4)
+
+BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility)
+{
+  /*
+   * utilities.cpp
+   */
+  bp::def("combineLaneletsShape", lanelet::utils::combineLaneletsShape);
+  bp::def(
+    "generateFineCenterline", lanelet::utils::generateFineCenterline,
+    generateFineCenterline_overload());
+  bp::def(
+    "getCenterlineWithOffset", lanelet::utils::getCenterlineWithOffset,
+    getCenterlineWithOffset_overload());
+  bp::def(
+    "getRightBoundWithOffset", lanelet::utils::getRightBoundWithOffset,
+    getRightBoundWithOffset_overload());
+  bp::def(
+    "getLeftBoundWithOffset", lanelet::utils::getLeftBoundWithOffset,
+    getLeftBoundWithOffset_overload());
+  bp::def("getExpandedLanelet", lanelet::utils::getExpandedLanelet);
+  bp::def("getExpandedLanelets", lanelet::utils::getExpandedLanelets);
+  bp::def(
+    "overwriteLaneletsCenterline", lanelet::utils::overwriteLaneletsCenterline,
+    overwriteLaneletsCenterline_overload());
+  bp::def("getConflictingLanelets", lanelet::utils::getConflictingLanelets);
+  bp::def("lineStringWithWidthToPolygon", ::lineStringWithWidthToPolygon);
+  bp::def("lineStringToPolygon", ::lineStringToPolygon);
+  bp::def<double(const lanelet::ConstLanelet &)>(
+    "getLaneletLength2d", lanelet::utils::getLaneletLength2d);
+  bp::def<double(const lanelet::ConstLanelet &)>(
+    "getLaneletLength3d", lanelet::utils::getLaneletLength3d);
+  bp::def<double(const lanelet::ConstLanelets &)>(
+    "getLaneletLength2d", lanelet::utils::getLaneletLength2d);
+  bp::def<double(const lanelet::ConstLanelets &)>(
+    "getLaneletLength3d", lanelet::utils::getLaneletLength3d);
+  bp::def("getArcCoordinates", ::getArcCoordinates);  // depends ros msg
+  bp::def("getClosestSegment", lanelet::utils::getClosestSegment);
+  bp::def("getPolygonFromArcLength", lanelet::utils::getPolygonFromArcLength);
+  bp::def("getLaneletAngle", ::getLaneletAngle);                  // depends on ros msg
+  bp::def("isInLanelet", ::isInLanelet, isInLanelet_overload());  // depends ros msg
+  bp::def("getClosestCenterPose", ::getClosestCenterPose);        // depends ros msg
+  // NOTE: required for the return-value of getClosestCenterPose
+  bp::class_<std::vector<double>>("[position, quaternion]")
+    .def(bp::vector_indexing_suite<std::vector<double>>());
+  bp::def("getLateralDistanceToCenterline", ::getLateralDistanceToCenterline);  // depends ros msg
+  bp::def(
+    "getLateralDistanceToClosestLanelet", ::getLateralDistanceToClosestLanelet);  // depends ros msg
+
+  /*
+   * query.cpp
+   */
+  bp::def("laneletLayer", lanelet::utils::query::laneletLayer);
+  bp::def("subtypeLanelets", ::subtypeLanelets);
+  bp::def("crosswalkLanelets", lanelet::utils::query::crosswalkLanelets);
+  bp::def("walkwayLanelets", lanelet::utils::query::walkwayLanelets);
+  bp::def("roadLanelets", lanelet::utils::query::roadLanelets);
+  bp::def("shoulderLanelets", lanelet::utils::query::shoulderLanelets);
+  bp::def("trafficLights", lanelet::utils::query::trafficLights);
+
+  bp::def("autowareTrafficLights", lanelet::utils::query::autowareTrafficLights);
+  converters::VectorToListConverter<std::vector<lanelet::AutowareTrafficLightConstPtr>>();
+
+  bp::def("detectionAreas", lanelet::utils::query::detectionAreas);
+  converters::VectorToListConverter<std::vector<lanelet::DetectionAreaConstPtr>>();
+
+  bp::def("noStoppingAreas", lanelet::utils::query::noStoppingAreas);
+  converters::VectorToListConverter<std::vector<lanelet::NoStoppingAreaConstPtr>>();
+
+  bp::def("noParkingAreas", lanelet::utils::query::noParkingAreas);
+  converters::VectorToListConverter<std::vector<lanelet::NoParkingAreaConstPtr>>();
+
+  bp::def("speedBumps", lanelet::utils::query::speedBumps);
+  converters::VectorToListConverter<std::vector<lanelet::SpeedBumpConstPtr>>();
+
+  bp::def("crosswalks", lanelet::utils::query::crosswalks);
+  converters::VectorToListConverter<std::vector<lanelet::CrosswalkConstPtr>>();
+
+  bp::def("curbstones", lanelet::utils::query::curbstones);
+  bp::def("getAllPolygonsByType", lanelet::utils::query::getAllPolygonsByType);
+  bp::def("getAllObstaclePolygons", lanelet::utils::query::getAllObstaclePolygons);
+  bp::def("getAllParkingLots", lanelet::utils::query::getAllParkingLots);
+  bp::def("getAllPartitions", lanelet::utils::query::getAllPartitions);
+  bp::def("getAllFences", lanelet::utils::query::getAllFences);
+  bp::def(
+    "getAllPedestrianPolygonMarkings", lanelet::utils::query::getAllPedestrianPolygonMarkings);
+  bp::def("getAllPedestrianLineMarkings", lanelet::utils::query::getAllPedestrianLineMarkings);
+  bp::def("getAllParkingSpaces", lanelet::utils::query::getAllParkingSpaces);
+
+  bp::def<lanelet::ConstLineStrings3d(
+    const lanelet::ConstLanelet &, const lanelet::LaneletMapConstPtr &)>(
+    "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces);
+  bp::def<lanelet::ConstLineStrings3d(
+    const lanelet::ConstLanelet &, const lanelet::ConstLineStrings3d &,
+    const lanelet::ConstPolygons3d &)>(
+    "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces);
+  // NOTE: required for iterating the return-value of getLinkedParkingSpaces/getAllParkingLots, but
+  // this causes RuntimeWarning for duplicate to-Python converter
+  bp::class_<lanelet::ConstLineStrings3d>("lanelet::ConstLineStrings3d")
+    .def(bp::vector_indexing_suite<lanelet::ConstLineStrings3d>());
+  bp::class_<lanelet::ConstPolygons3d>("lanelet::ConstPolygons3d")
+    .def(bp::vector_indexing_suite<lanelet::ConstPolygons3d>());
+
+  bp::def<lanelet::Optional<lanelet::ConstLanelet>(
+    const lanelet::ConstLineString3d &, const lanelet::ConstLanelets &,
+    const lanelet::ConstPolygons3d &)>("getLinkedLanelet", ::getLinkedLanelet);
+  bp::def<lanelet::Optional<lanelet::ConstLanelet>(
+    const lanelet::ConstLineString3d &, const lanelet::LaneletMapConstPtr &)>(
+    "getLinkedLanelet", ::getLinkedLanelet);
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::ConstLineString3d &, const lanelet::ConstLanelets &,
+    const lanelet::ConstPolygons3d &)>(
+    "getLinkedLanelets", lanelet::utils::query::getLinkedLanelets);
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::ConstLineString3d &, const lanelet::LaneletMapConstPtr &)>(
+    "getLinkedLanelets", lanelet::utils::query::getLinkedLanelets);
+  bp::def<lanelet::Optional<lanelet::ConstPolygon3d>(
+    const lanelet::ConstLanelet &, const lanelet::ConstPolygons3d &)>(
+    "getLinkedParkingLot", ::getLinkedParkingLot);
+  bp::def<lanelet::Optional<lanelet::ConstPolygon3d>(
+    const lanelet::BasicPoint2d &, const lanelet::ConstPolygons3d &)>(
+    "getLinkedParkingLot", ::getLinkedParkingLot);
+  bp::def<lanelet::Optional<lanelet::ConstPolygon3d>(
+    const lanelet::ConstLineString3d &, const lanelet::ConstPolygons3d &)>(
+    "getLinkedParkingLot", ::getLinkedParkingLot);
+  bp::def<lanelet::ConstLineStrings3d(
+    const lanelet::ConstPolygon3d & parking_lot,
+    const lanelet::ConstLineStrings3d & all_parking_spaces)>(
+    "getLinkedParkingSpaces", lanelet::utils::query::getLinkedParkingSpaces);
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets)>(
+    "getLinkedLanelets", lanelet::utils::query::getLinkedLanelets);
+  bp::def("stopLinesLanelets", lanelet::utils::query::stopLinesLanelets);
+  bp::def("stopLinesLanelet", lanelet::utils::query::stopLinesLanelet);
+  bp::def(
+    "stopSignStopLines", lanelet::utils::query::stopSignStopLines, stopSignStopLines_overload());
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::ConstLanelets &, const lanelet::BasicPoint2d &, const double)>(
+    "getLaneletsWithinRange", lanelet::utils::query::getLaneletsWithinRange);
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::ConstLanelets &, const std::string &, const double)>(
+    "getLaneletsWithinRange_point", ::getLaneletsWithinRange_point);  // depends on ros msg
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::routing::RoutingGraphPtr &, const lanelet::ConstLanelet &)>(
+    "getLaneChangeableNeighbors", lanelet::utils::query::getLaneChangeableNeighbors);
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::routing::RoutingGraphPtr &, const lanelet::ConstLanelets &,
+    const std::string &)>(
+    "getLaneChangeableNeighbors_point", ::getLaneChangeableNeighbors_point);  // depends on ros msg
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::routing::RoutingGraphPtr &, const lanelet::ConstLanelet &)>(
+    "getAllNeighbors", lanelet::utils::query::getAllNeighbors);
+  bp::def<lanelet::ConstLanelets(
+    const lanelet::routing::RoutingGraphPtr &, const lanelet::ConstLanelets &,
+    const std::string &)>("getAllNeighbors_point", ::getAllNeighbors_point);  // depends on ros msg
+  bp::def("getAllNeighborsLeft", lanelet::utils::query::getAllNeighborsLeft);
+  bp::def("getAllNeighborsRight", lanelet::utils::query::getAllNeighborsRight);
+  bp::def("getClosestLanelet", ::getClosestLanelet);  // depends on ros msg
+  bp::def(
+    "getClosestLaneletWithConstrains", ::getClosestLaneletWithConstrains,
+    getClosestLaneletWithConstrains_overload());                    // depends on ros msg
+  bp::def("getCurrentLanelets_point", ::getCurrentLanelets_point);  // depends on ros msg
+  bp::def("getCurrentLanelets_pose", ::getCurrentLanelets_pose);    // depends on ros msg
+  // NOTE: this is required for iterating getCurrentLanelets return value directly
+  bp::class_<lanelet::ConstLanelets>("lanelet::ConstLanelets")
+    .def(bp::vector_indexing_suite<lanelet::ConstLanelets>());
+  // NOTE: this is required for return-type of getSucceeding/PrecedingLaneletSequences
+  bp::class_<std::vector<lanelet::ConstLanelets>>("std::vector<lanelet::ConstLanelets>")
+    .def(bp::vector_indexing_suite<std::vector<lanelet::ConstLanelets>>());
+  bp::def("getSucceedingLaneletSequences", lanelet::utils::query::getSucceedingLaneletSequences);
+  bp::def(
+    "getPrecedingLaneletSequences", lanelet::utils::query::getPrecedingLaneletSequences,
+    getPrecedingLaneletSequences_overload());
+}
+
+// NOLINTEND(readability-identifier-naming)
diff --git a/setup.cfg b/setup.cfg
new file mode 100644
index 0000000..5214751
--- /dev/null
+++ b/setup.cfg
@@ -0,0 +1,15 @@
+[flake8]
+# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini
+extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000
+import-order-style = pep8
+max-line-length = 100
+show-source = true
+statistics = true
+
+[isort]
+profile=black
+line_length=100
+force_sort_within_sections=true
+force_single_line=true
+reverse_relative=true
+known_third_party=launch