Implement robot_localization

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]

src/igvc_slam/launch/sim_rtabmap.launch.py

@@ -1,49 +1,149 @@
 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',
-            '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
-    ])
+        #stereo_odom_node,
+        #icp_odom_node,
+        rtabmap,
+        robot_localization_node
+    ])
+
+

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
+    ])
+
+