diff --git a/src/igvc_slam/config/ekf.yaml b/src/igvc_slam/config/ekf.yaml
new file mode 100644
index 00000000..1b3da77d
--- /dev/null
+++ b/src/igvc_slam/config/ekf.yaml
@@ -0,0 +1,60 @@
+# Note, this is copied straight from the robot_localization documentation
+# This is a more detailed example of a ekf.yaml setup: https://github.com/cra-ros-pkg/robot_localization/blob/rolling-devel/params/ekf.yaml
+
+### ekf config file ###
+ekf_filter_node:
+ ros__parameters:
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+ frequency: 30.0
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+ two_d_mode: false
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+ publish_acceleration: true
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+ publish_tf: true
+
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
+# to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
+# observations) then:
+# 3a. Set your "world_frame" to your map_frame value
+# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
+# from robot_localization! However, that instance should *not* fuse the global data.
+ map_frame: map # Defaults to "map" if unspecified
+ odom_frame: odom # Defaults to "odom" if unspecified
+ base_link_frame: base_link # Defaults to "base_link" if unspecified
+ world_frame: odom # Defaults to the value of odom_frame if unspecified
+
+ # Assumes icp_odometry is not publishing to odom
+ odom0: rtabmap/odom_icp
+ odom0_config: [false, false, false,
+ false, false, false,
+ true, true, false,
+ false, false, true,
+ false, false, false]
+
+ # Assumes icp_odometry is not publishing to odom
+ odom1: rtabmap/odom_stereo
+ odom01_config: [false, false, false,
+ false, false, false,
+ true, true, false,
+ false, false, true,
+ false, false, false]
+
+ # Note I'm not sure what topic our imu is
+ imu0: /imu
+ imu0_config: [false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, true,
+ false, false, false]
\ No newline at end of file
diff --git a/src/igvc_slam/launch/sim_rtabmap.launch.py b/src/igvc_slam/launch/sim_rtabmap.launch.py
index 41b3991f..42dca047 100644
--- a/src/igvc_slam/launch/sim_rtabmap.launch.py
+++ b/src/igvc_slam/launch/sim_rtabmap.launch.py
@@ -1,15 +1,15 @@
import os
from ament_index_python.packages import get_package_share_directory
-
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
-
+from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
def generate_launch_description():
rtabmap_package = get_package_share_directory('rtabmap_launch')
+ package_slam = FindPackageShare(package='igvc_slam').find('igvc_slam')
rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py')
@@ -17,33 +17,133 @@ def generate_launch_description():
PythonLaunchDescriptionSource(rtabmap_launch_path),
launch_arguments={
'args' : '--delete_db_on_start',
+ # Topics
'depth_topic' : '/camera/camera/depth/image_rect_raw',
'rgb_topic' : '/camera/camera/color/image_raw',
'camera_info_topic' : '/camera/camera/depth/camera_info',
- 'approx_sync' : 'true',
+ 'imu_topic' : '/rtabmap/imu', # Placeholder path
+ 'gps_topic' : '/rtabmap/gps', # Placeholder path
+ 'odom_topic' : '/odom', # Filtered odometry from robot_localization
+ 'scan_topic' : '/scan', # LiDAR placeholder
+ 'map_topic' : '/map',
+
+ # Camera Arguments
'frame_id' : 'base_footprint',
- 'log_level' : 'debug',
- 'publish_tf_odom' : 'true',
- 'odom_topic' : '/odom',
- 'odom_frame_id' : 'odom',
- 'sync_queue_size' : '10', #TODO verify
- 'topic_queue_size' : '10', #TODO verify
'rgbd_sync' : 'true',
- 'approx_rgbd_sync' : 'true',
- 'subscribe_depth' : 'false',
- 'subscribe_rgbd' : 'true',
- 'visual_odometry' : 'true',
+ 'approx_rgbd_sync' : 'true',
'approx_sync_max_interval' : '0.01',
- 'qos' : '1',
- 'rviz' : 'true',
- 'use_sim_time' : 'true',
- 'wait_for_transform' : '0.4',
+ 'subscribe_rgbd' : 'true',
+ 'subscribe_depth' : 'false',
+
+ # Odometry Arguments
+ 'odom_frame_id' : 'odom',
+ 'publish_tf_odom' : 'true',
+ 'icp_odometry' : 'false', # Lidar Odom
+ 'visual_odometry' : 'false', # Visual Odom
'imu_topic' : '/rtabmap/imu',
'wait_imu_to_init' : 'false',
- 'map_topic' : '/map'
+
+ # LiDAR Arguments
+ 'subscribe_scan' : 'true',
+
+ # Misc.
+ 'log_level' : 'debug',
+ 'approx_sync' : 'true',
+ 'qos' : '1',
+ 'sync_queue_size' : '50',
+ 'topic_queue_size' : '50',
+ 'wait_for_transform' : '0.4',
+ #'rtabmap_viz' : 'true',
}.items()
)
+
+ robot_localization_node = Node(
+ package='robot_localization',
+ executable='ekf_node',
+ name='ekf_filter_node',
+ output='screen',
+ parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}]
+ )
+
+ # # Stereo odometry
+ # stereo_odom_node = Node(
+ # package='rtabmap_odom', executable='stereo_odometry', name='stereo_odometry', output='screen',
+ # emulate_tty=True,
+ # parameters=[{
+ # 'frame_id' : 'frame_id',
+ # 'odom_frame_id' : 'vo_frame_id',
+ # 'publish_tf' : 'publish_tf_odom',
+ # 'ground_truth_frame_id' : 'ground_truth_frame_id',
+ # 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id',
+ # 'wait_for_transform' : 'wait_for_transform',
+ # 'wait_imu_to_init' : 'wait_imu_to_init',
+ # 'always_check_imu_tf' : 'always_check_imu_tf',
+ # 'approx_sync' : 'approx_sync',
+ # 'approx_sync_max_interval' : 'approx_sync_max_interval',
+ # 'config_path' : 'cfg',
+ # 'topic_queue_size' : 'topic_queue_size',
+ # 'sync_queue_size' : 'sync_queue_size',
+ # 'qos' : 'qos_image',
+ # 'qos_camera_info' : 'qos_camera_info',
+ # 'qos_imu' : 'qos_imu',
+ # 'subscribe_rgbd' : 'subscribe_rgbd',
+ # 'guess_frame_id' : 'odom_guess_frame_id',
+ # 'guess_min_translation' : 'odom_guess_min_translation',
+ # 'guess_min_rotation' : 'odom_guess_min_rotation'}],
+ # remappings=[{
+ # 'left_image_topic_relay' : '/left/image_rect',
+ # 'right_image_topic_relay' : '/right/image_rect',
+ # 'left_camera_info_topic' : '/left/camera_info',
+ # 'right_camera_info_topic' : '/right/camera_info',
+ # 'rgbd_topic_relay' : '/rgbd_image',
+ # 'odom_topic' : '/odom_stereo', # Hopefully changes where its publishing
+ # 'imu_topic' : '/imu'}],
+ # arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.stereo_odometry:=', 'odom_log_level', "--log-level", 'stereo_odometry:=', 'odom_log_level'],
+ # prefix= 'launch_prefix',
+ # namespace= 'namespace'
+ # ),
+
+ # # ICP odometry
+ # icp_odom_node = Node(
+ # package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
+ # name='icp_odometry_node',
+ # emulate_tty=True,
+ # parameters=[{
+ # 'frame_id' : 'frame_id',
+ # 'odom_frame_id' : 'vo_frame_id',
+ # 'publish_tf' : 'publish_tf_odom',
+ # 'ground_truth_frame_id' : 'ground_truth_frame_id',
+ # 'ground_truth_base_frame_id' : 'ground_truth_base_frame_id',
+ # 'wait_for_transform' : 'wait_for_transform',
+ # 'wait_imu_to_init' : 'wait_imu_to_init',
+ # 'always_check_imu_tf' : 'always_check_imu_tf',
+ # 'approx_sync' : 'approx_sync',
+ # 'approx_sync_max_interval' : 'approx_sync_max_interval',
+ # 'config_path' : 'cfg',
+ # 'topic_queue_size' : 'topic_queue_size',
+ # 'sync_queue_size' : 'sync_queue_size',
+ # 'qos' : 'qos_image',
+ # 'qos_camera_info' : 'qos_camera_info',
+ # 'qos_imu' : 'qos_imu',
+ # 'subscribe_rgbd' : 'subscribe_rgbd',
+ # 'guess_frame_id' : 'odom_guess_frame_id',
+ # 'guess_min_translation' : 'odom_guess_min_translation',
+ # 'guess_min_rotation' : 'odom_guess_min_rotation'}],
+ # remappings=[{
+ # 'scan_topic' : '/scan',
+ # 'scan_cloud_topic' : '/scan_cloud',
+ # 'odom_topic' : '/odom_icp', # Hopefully changes where its publishing
+ # 'imu_topic' : '/imu'}],
+ # arguments=['args', 'odom_args', "--ros-args", "--log-level", 'namespace', '.icp_odometry:=', 'odom_log_level'],
+ # prefix= 'launch_prefix',
+ # namespace= 'namespace'
+ # ),
return LaunchDescription([
- rtabmap
- ])
\ No newline at end of file
+ #stereo_odom_node,
+ #icp_odom_node,
+ rtabmap,
+ robot_localization_node
+ ])
+
+
\ No newline at end of file
diff --git a/src/igvc_slam/launch/sim_rtabmap.launch_Mar16.py b/src/igvc_slam/launch/sim_rtabmap.launch_Mar16.py
new file mode 100644
index 00000000..664ce56f
--- /dev/null
+++ b/src/igvc_slam/launch/sim_rtabmap.launch_Mar16.py
@@ -0,0 +1,152 @@
+# WORK FILE
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ rtabmap_package = get_package_share_directory('rtabmap_launch')
+ package_slam = FindPackageShare(package='igvc_slam').find('igvc_slam')
+
+ rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py')
+
+ rtabmap = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(rtabmap_launch_path),
+ launch_arguments={
+ 'args' : '--delete_db_on_start',
+ # Topics
+ 'depth_topic' : '/camera/camera/depth/image_rect_raw',
+ 'rgb_topic' : '/camera/camera/color/image_raw',
+ 'camera_info_topic' : '/camera/camera/depth/camera_info',
+ 'imu_topic' : '/rtabmap/imu', # Placeholder path
+ 'gps_topic' : '/rtabmap/gps', # Placeholder path
+ 'odom_topic' : '/odom', # Filtered odometry from robot_localization
+ 'scan_topic' : '/scan', # LiDAR placeholder
+ 'map_topic' : '/map',
+
+ # Camera Arguments
+ 'frame_id' : 'base_footprint',
+ 'rgbd_sync' : 'true',
+ 'approx_rgbd_sync' : 'true',
+ 'approx_sync_max_interval' : '0.01',
+ 'subscribe_rgbd' : 'true',
+ 'subscribe_depth' : 'false',
+
+ # Odometry Arguments
+ 'odom_frame_id' : 'odom',
+ 'publish_tf_odom' : 'true',
+ 'icp_odometry' : 'false', # Lidar Odom
+ 'visual_odometry' : 'false', # Visual Odom
+ 'imu_topic' : '/rtabmap/imu',
+ 'wait_imu_to_init' : 'false',
+
+ # LiDAR Arguments
+ 'subscribe_scan' : 'true',
+
+ # Misc.
+ 'log_level' : 'debug',
+ 'approx_sync' : 'true',
+ 'qos' : '1',
+ 'sync_queue_size' : '50',
+ 'topic_queue_size' : '50',
+ 'wait_for_transform' : '0.4',
+ 'rtabmap_viz' : 'true',
+ }.items()
+ )
+
+ robot_localization_node = Node(
+ package='robot_localization',
+ executable='ekf_node',
+ name='ekf_filter_node',
+ output='screen',
+ parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}]
+ )
+
+
+ # Stereo odometry
+ stereo_odom_node = Node(
+ package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen",
+ emulate_tty=True,
+ condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])),
+ parameters=[{
+ "frame_id": LaunchConfiguration('frame_id'),
+ "odom_frame_id": LaunchConfiguration('vo_frame_id'),
+ "publish_tf": LaunchConfiguration('publish_tf_odom'),
+ "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
+ "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
+ "wait_for_transform": LaunchConfiguration('wait_for_transform'),
+ "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
+ "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
+ "approx_sync": LaunchConfiguration('approx_sync'),
+ "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
+ "config_path": LaunchConfiguration('cfg').perform(context),
+ "topic_queue_size": LaunchConfiguration('topic_queue_size'),
+ "sync_queue_size": LaunchConfiguration('sync_queue_size'),
+ "qos": LaunchConfiguration('qos_image'),
+ "qos_camera_info": LaunchConfiguration('qos_camera_info'),
+ "qos_imu": LaunchConfiguration('qos_imu'),
+ "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
+ "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
+ "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
+ "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+ remappings=[
+ ("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
+ ("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
+ ("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
+ ("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
+ ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
+ ("odom", LaunchConfiguration('odom_topic')),
+ ("imu", LaunchConfiguration('imu_topic'))],
+ arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]],
+ prefix=LaunchConfiguration('launch_prefix'),
+ namespace=LaunchConfiguration('namespace')
+ ),
+
+
+ # ICP odometry
+ icp_odom_node = Node(
+ package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
+ emulate_tty=True,
+ condition=IfCondition(LaunchConfiguration('icp_odometry')),
+ parameters=[{
+ "frame_id": LaunchConfiguration('frame_id'),
+ "odom_frame_id": LaunchConfiguration('vo_frame_id'),
+ "publish_tf": LaunchConfiguration('publish_tf_odom'),
+ "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
+ "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
+ "wait_for_transform": LaunchConfiguration('wait_for_transform'),
+ "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
+ "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
+ "approx_sync": LaunchConfiguration('approx_sync'),
+ "config_path": LaunchConfiguration('cfg').perform(context),
+ "topic_queue_size": LaunchConfiguration('topic_queue_size'),
+ "sync_queue_size": LaunchConfiguration('sync_queue_size'),
+ "qos": LaunchConfiguration('qos_image'),
+ "qos_imu": LaunchConfiguration('qos_imu'),
+ "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
+ "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
+ "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+ remappings=[
+ ("scan", LaunchConfiguration('scan_topic')),
+ ("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
+ ("odom", LaunchConfiguration('odom_topic')),
+ ("imu", LaunchConfiguration('imu_topic'))],
+ arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]],
+ prefix=LaunchConfiguration('launch_prefix'),
+ namespace=LaunchConfiguration('namespace')
+ ),
+
+ return LaunchDescription([
+ #stereo_odom_node,
+ #icp_odom_node,
+ rtabmap,
+ robot_localization_node
+ ])
+
+
\ No newline at end of file
diff --git a/src/igvc_slam/launch/sim_rtabmap.launch_rtabmap_experiemnt.py b/src/igvc_slam/launch/sim_rtabmap.launch_rtabmap_experiemnt.py
new file mode 100644
index 00000000..08d9d67b
--- /dev/null
+++ b/src/igvc_slam/launch/sim_rtabmap.launch_rtabmap_experiemnt.py
@@ -0,0 +1,171 @@
+# WORK FILE
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+#from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+# From rtabmap launch file
+
+from launch import LaunchDescription, Substitution, LaunchContext
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PythonExpression
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.actions import SetParameter
+from typing import Text
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+ rtabmap_package = get_package_share_directory('rtabmap_launch')
+ package_slam = FindPackageShare(package='igvc_slam').find('igvc_slam')
+
+ rtabmap_launch_path = os.path.join(rtabmap_package, 'launch', 'rtabmap.launch.py')
+
+ rtabmap = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(rtabmap_launch_path),
+ launch_arguments={
+ 'args' : '--delete_db_on_start',
+ # Topics
+ 'depth_topic' : '/camera/camera/depth/image_rect_raw',
+ 'rgb_topic' : '/camera/camera/color/image_raw',
+ 'camera_info_topic' : '/camera/camera/depth/camera_info',
+ 'imu_topic' : '/rtabmap/imu', # Placeholder path
+ 'gps_topic' : '/rtabmap/gps', # Placeholder path
+ 'odom_topic' : '/odom', # Filtered odometry from robot_localization
+ 'scan_topic' : '/scan', # LiDAR placeholder
+ 'map_topic' : '/map',
+
+ # Camera Arguments
+ 'frame_id' : 'base_footprint',
+ 'rgbd_sync' : 'true',
+ 'approx_rgbd_sync' : 'true',
+ 'approx_sync_max_interval' : '0.01',
+ 'subscribe_rgbd' : 'true',
+ 'subscribe_depth' : 'false',
+
+ # Odometry Arguments
+ 'odom_frame_id' : 'odom',
+ 'publish_tf_odom' : 'true',
+ 'icp_odometry' : 'false', # Lidar Odom,
+ 'visual_odometry' : 'false', # Visual Odom, published topic 'rgbd_odometry'
+ 'imu_topic' : '/rtabmap/imu',
+ 'wait_imu_to_init' : 'false',
+
+ # LiDAR Arguments
+ 'subscribe_scan' : 'true',
+
+ # Misc.
+ 'log_level' : 'debug',
+ 'approx_sync' : 'true',
+ 'qos' : '1',
+ 'sync_queue_size' : '50',
+ 'topic_queue_size' : '50',
+ 'wait_for_transform' : '0.4',
+ 'rtabmap_viz' : 'true',
+ }.items()
+ )
+
+ robot_localization_node = Node(
+ package='robot_localization',
+ executable='ekf_node',
+ name='ekf_filter_node',
+ output='screen',
+ parameters=[os.path.join(package_slam, 'config/ekf.yaml'), {'use_sim_time': 'true'}]
+ )
+
+
+ # Stereo odometry
+
+ # Stereo odometry
+ stereo_odom_node = Node(
+ package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen",
+ emulate_tty=True,
+ condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])),
+ parameters=[{
+ "frame_id": LaunchConfiguration('frame_id'),
+ "odom_frame_id": LaunchConfiguration('vo_frame_id'),
+ "publish_tf": LaunchConfiguration('publish_tf_odom'),
+ "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
+ "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
+ "wait_for_transform": LaunchConfiguration('wait_for_transform'),
+ "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
+ "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
+ "approx_sync": LaunchConfiguration('approx_sync'),
+ "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
+ "config_path": LaunchConfiguration('cfg').perform(context),
+ "topic_queue_size": LaunchConfiguration('topic_queue_size'),
+ "sync_queue_size": LaunchConfiguration('sync_queue_size'),
+ "qos": LaunchConfiguration('qos_image'),
+ "qos_camera_info": LaunchConfiguration('qos_camera_info'),
+ "qos_imu": LaunchConfiguration('qos_imu'),
+ "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
+ "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
+ "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
+ "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+ remappings=[
+ ("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
+ ("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
+ ("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
+ ("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
+ ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
+ ("odom", LaunchConfiguration('odom_topic')),
+ ("imu", LaunchConfiguration('imu_topic'))],
+ arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]],
+ prefix=LaunchConfiguration('launch_prefix'),
+ namespace=LaunchConfiguration('namespace')
+ ),
+
+
+ # ICP odometry
+ icp_odom_node = Node(
+ package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
+ emulate_tty=True,
+ condition=IfCondition(LaunchConfiguration('icp_odometry')),
+ parameters=[{
+ "frame_id": LaunchConfiguration('frame_id'),
+ "odom_frame_id": LaunchConfiguration('vo_frame_id'),
+ "publish_tf": LaunchConfiguration('publish_tf_odom'),
+ "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
+ "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
+ "wait_for_transform": LaunchConfiguration('wait_for_transform'),
+ "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
+ "always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
+ "approx_sync": LaunchConfiguration('approx_sync'),
+ "config_path": LaunchConfiguration('cfg').perform(context),
+ "topic_queue_size": LaunchConfiguration('topic_queue_size'),
+ "sync_queue_size": LaunchConfiguration('sync_queue_size'),
+ "qos": LaunchConfiguration('qos_image'),
+ "qos_imu": LaunchConfiguration('qos_imu'),
+ "guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
+ "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
+ "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+ remappings=[
+ ("scan", LaunchConfiguration('scan_topic')),
+ ("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
+ ("odom", LaunchConfiguration('odom_topic')),
+ ("imu", LaunchConfiguration('imu_topic'))],
+ arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]],
+ prefix=LaunchConfiguration('launch_prefix'),
+ namespace=LaunchConfiguration('namespace')
+ ),
+
+ def perform(self, context: 'LaunchContext') -> Text:
+ if self.condition == True or self.condition == 'true' or self.condition == 'True':
+ return self.text_if
+ else:
+ return self.text_else
+
+ return LaunchDescription([
+ #stereo_odom_node,
+ #icp_odom_node,
+ rtabmap,
+ robot_localization_node
+ ])
+
+
\ No newline at end of file
diff --git a/src/igvc_slam/package.xml b/src/igvc_slam/package.xml
index 9dc74bc8..c62cb168 100644
--- a/src/igvc_slam/package.xml
+++ b/src/igvc_slam/package.xml
@@ -14,6 +14,7 @@
rtabmap_ros
imu_filter_madgwick
+ robot_localization
ament_cmake