initial commit

2022-08-05 08:23:25 +03:00
commit 5ecdc6abcf
387 changed files with 3010095 additions and 0 deletions
--- a/ov_msckf/launch/subscribe.launch.py
+++ b/ov_msckf/launch/subscribe.launch.py
@@ -0,0 +1,114 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
+import os
+import sys
+
+launch_args = [
+    DeclareLaunchArgument(name="namespace", default_value="", description="namespace"),
+    DeclareLaunchArgument(
+        name="ov_enable", default_value="true", description="enable OpenVINS node"
+    ),
+    DeclareLaunchArgument(
+        name="rviz_enable", default_value="true", description="enable rviz node"
+    ),
+    DeclareLaunchArgument(
+        name="config",
+        default_value="euroc_mav",
+        description="euroc_mav, tum_vi, rpng_aruco...",
+    ),
+    DeclareLaunchArgument(
+        name="config_path",
+        default_value="",
+        description="path to estimator_config.yaml. If not given, determined based on provided 'config' above",
+    ),
+    DeclareLaunchArgument(
+        name="verbosity",
+        default_value="INFO",
+        description="ALL, DEBUG, INFO, WARNING, ERROR, SILENT",
+    ),
+    DeclareLaunchArgument(
+        name="use_stereo",
+        default_value="true",
+        description="if we have more than 1 camera, if we should try to track stereo constraints between pairs",
+    ),
+    DeclareLaunchArgument(
+        name="max_cameras",
+        default_value="2",
+        description="how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)",
+    ),
+    DeclareLaunchArgument(
+        name="save_total_state",
+        default_value="false",
+        description="record the total state with calibration and features to a txt file",
+    )
+]
+
+def launch_setup(context):
+    config_path = LaunchConfiguration("config_path").perform(context)
+    if not config_path:
+        configs_dir = os.path.join(get_package_share_directory("ov_msckf"), "config")
+        available_configs = os.listdir(configs_dir)
+        config = LaunchConfiguration("config").perform(context)
+        if config in available_configs:
+            config_path = os.path.join(
+                            get_package_share_directory("ov_msckf"),
+                            "config",config,"estimator_config.yaml"
+                        )
+        else:
+            return [
+                LogInfo(
+                    msg="ERROR: unknown config: '{}' - Available configs are: {} - not starting OpenVINS".format(
+                        config, ", ".join(available_configs)
+                    )
+                )
+            ]
+    else:
+        if not os.path.isfile(config_path):
+            return [
+                LogInfo(
+                    msg="ERROR: config_path file: '{}' - does not exist. - not starting OpenVINS".format(
+                        config_path)
+                    )
+            ]
+    node1 = Node(
+        package="ov_msckf",
+        executable="run_subscribe_msckf",
+        condition=IfCondition(LaunchConfiguration("ov_enable")),
+        namespace=LaunchConfiguration("namespace"),
+        output='screen',
+        parameters=[
+            {"verbosity": LaunchConfiguration("verbosity")},
+            {"use_stereo": LaunchConfiguration("use_stereo")},
+            {"max_cameras": LaunchConfiguration("max_cameras")},
+            {"save_total_state": LaunchConfiguration("save_total_state")},
+            {"config_path": config_path},
+        ],
+    )
+
+    node2 = Node(
+        package="rviz2",
+        executable="rviz2",
+        condition=IfCondition(LaunchConfiguration("rviz_enable")),
+        arguments=[
+            "-d"
+            + os.path.join(
+                get_package_share_directory("ov_msckf"), "launch", "display_ros2.rviz"
+            ),
+            "--ros-args",
+            "--log-level",
+            "warn",
+            ],
+    )
+
+    return [node1, node2]
+
+
+def generate_launch_description():
+    opfunc = OpaqueFunction(function=launch_setup)
+    ld = LaunchDescription(launch_args)
+    ld.add_action(opfunc)
+    return ld