a

config/ekf.yaml

+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: 50.0
+
+        # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+        # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+        # filter will generate new output. Defaults to 1 / frequency if not specified.
+        sensor_timeout: 0.1
+
+        # 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: true
+
+        # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+        # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+        # unspecified.
+        transform_time_offset: 0.0
+
+        # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
+        # Defaults to 0.0 if unspecified.
+        transform_timeout: 0.0
+
+        # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+        # unhappy with any settings or data.
+        print_diagnostics: true
+
+
+        # Whether we'll allow old measurements to cause a re-publication of the updated state
+        permit_corrected_publication: false
+
+        # Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: false
+
+        # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+        publish_tf: true
+
+        # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+        # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+        # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+        # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+        # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+        # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+        # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+        # ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+        # Here is how to use the following settings:
+        # 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_footprint  # Defaults to "base_link" if unspecified
+        world_frame: odom           # Defaults to the value of odom_frame if unspecified
+
+        # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+        # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+        # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+        # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+        # default values, and must be specified.
+        odom0: encoder_odometry
+
+        # Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+        # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+        # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+        # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+        # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+        # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+        # if unspecified, effectively making this parameter required for each sensor.
+        odom0_config: [true,  true,  false,
+                       false, false, false,
+                       false, false, false,
+                       false, false, true,
+                       false, false, false]
+
+
+        # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+        # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+        # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+        # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+        # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+        # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+        # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+        # for twist measurements has no effect.
+        odom0_differential: false
+
+
+
+        imu0: Imu
+        imu0_config: [false, false, false,
+                      true,  true,  true,
+                      false, false, false,
+                      true,  true,  true,
+                      true,  true,  true]
+        imu0_differential: false
\ No newline at end of file

launch/test.launch.py

+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, Command
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+
+    # Process the URDF file
+    pkg_path = os.path.join(get_package_share_directory('robot_loc'))
+    xacro_file = os.path.join(pkg_path,'urdf','robomaster_ep.urdf.xacro')
+    # robot_description_config = xacro.process_file(xacro_file).toxml()
+    robot_description_config = Command(['xacro ', xacro_file])
+    
+    # Create a robot_state_publisher node
+    params = {'robot_description': robot_description_config}
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+
+    
+    node_lidar = Node(
+            package='rplidar_ros',
+            executable='rplidar_composition',
+            output='screen',
+            parameters=[{
+                'serial_port': '/dev/serial/by-path/platform-fd500000.pcie-pci-0000:01:00.0-usb-0:1.2:1.0-port0',
+                'frame_id': 'laser_link',
+                'angle_compensate': True,
+                'scan_mode': 'Sensitivity'
+            }]
+        )
+    
+    node_odom = Node(
+            package='robomaster_pi',
+            executable='Odom',
+        )
+    
+    node_vel = Node(
+            package='robomaster_pi',
+            executable='conn_vel',
+        )
+    
+    node_IMU = Node(
+            package='robomaster_pi',
+            executable='IMU',
+        )
+    
+    node_ekf = Node(
+            package='robot_localization',
+            executable='ekf_node',
+            name='ekf_filter_node',
+            output='screen',
+            parameters=[os.path.join(get_package_share_directory("robot_loc"), 'config', 'ekf.yaml')]
+    )
+
+    
+
+
+    # Launch!
+    return LaunchDescription([
+        node_robot_state_publisher,
+        node_lidar,
+        node_odom,
+        node_vel,
+        node_IMU,
+        node_ekf
+        
+    ])