Adding initial localization package

src/subjugator/gnc/subjugator_localization/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.8)
+project(subjugator_localization)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Install launch files.
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

src/subjugator/gnc/subjugator_localization/launch/subjugator_localization.launch.py

@@ -0,0 +1,94 @@
+from dataclasses import dataclass
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+AvailableTypes = str | list | float | int
+
+
+@dataclass
+class Parameter:
+    name: str
+    value: AvailableTypes
+
+    def dict(self) -> dict[str, AvailableTypes]:
+        return {self.name: self.value}
+
+
+def generate_launch_description():
+    parameters: list[Parameter] = [
+        Parameter("imu0", "/imu/data_raw"),
+        Parameter(
+            "imu0_config",
+            [
+                False,
+                False,
+                False,
+                True,
+                True,
+                True,
+                False,
+                False,
+                False,
+                True,
+                True,
+                False,
+                True,
+                True,
+                True,
+            ],
+        ),
+        Parameter("imu0_differential", False),
+        Parameter("imu0_relative", True),
+        Parameter("imu0_remove_gravitational_acceleration", True),
+        Parameter("imu0_queue_size", 10),
+        #        Parameter("odom0", "/dvl/odom"),
+        #        Parameter(
+        #            "odom0_config",
+        #            [
+        #                True,
+        #                True,
+        #                False,
+        #                False,
+        #                False,
+        #                True,
+        #                True,
+        #                False,
+        #                False,
+        #                False,
+        #                False,
+        #                True,
+        #                False,
+        #                False,
+        #                False,
+        #            ],
+        #        ),
+        #        Parameter("odom0_differential", True),
+        #        Parameter("odom0_relative", True),
+        #        Parameter("odom0_queue_size", 10),
+        #        Parameter("pose0", "/depth/pose"),
+        #        Parameter("pose0_config", [False, False, True,
+        #                                   False, False, False,
+        #                                   False, False, False,
+        #                                   False, False, False,
+        #                                   False, False, False]),
+        #        Parameter("pose0_differential", True),
+        #        Parameter("pose0_relative", True),
+        #        Parameter("pose0_queue_size", 10),
+        Parameter("print_diagnostics", True),
+        Parameter("publish_acceleration", True),
+    ]
+    import rich
+
+    rich.print([p.dict() for p in parameters])
+    return LaunchDescription(
+        [
+            Node(
+                package="robot_localization",
+                namespace="subjugator_localization",
+                executable="ekf_node",
+                name="ekf_filter_node",
+                parameters=[p.dict() for p in parameters],
+            ),
+        ],
+    )

src/subjugator/gnc/subjugator_localization/package.xml

@@ -0,0 +1,20 @@
+<?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>subjugator_localization</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="alexjohnson@ufl.edu">exla</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>