Position seems to be offsetted in Eagleye. What could be causing this?

[sisaha9]

The yellow dot in the below video is the ground truth and the blue dot is the estimated location provided by Eagleye. I am using the autoware-main branch

Here is the parameter file being used

/**: #GNSS cycle 5Hz, IMU cycle 50Hz.
  ros__parameters:
    # Estimate mode
    use_gnss_mode: rtklib
    use_can_less_mode: false
    use_multi_antenna_mode: true

    # Topic
    twist:
      twist_type: 0 # TwistStamped : 0, TwistWithCovarianceStamped: 1
      twist_topic: /eagleye_input/can_twist
    imu_topic: /novatel_interface/imu
    gnss:
      velocity_source_type: 3 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3
      velocity_source_topic: /novatel_interface/twist
      llh_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
      llh_source_topic: /novatel_interface/fix
    sub_gnss:
      llh_source_type: 2 # nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
      llh_source_topic:  /sensing/sub_gnss/ublox/nav_sat_fix

    # TF
    tf_gnss_frame:
      parent: "base_link"
      child: "novatel"

    # Origin of GNSS coordinates (ECEF to ENU)
    ecef_base_pos:
      x: 0.0
      y: 0.0
      z: 0.0
      use_ecef_base_position: false

    reverse_imu_wz: false

    # Navigation Parameters
    # Basic Navigation Functions
    common:
      imu_rate: 100
      gnss_rate: 20
      stop_judgment_threshold: 0.01
      slow_judgment_threshold: 0.278
      moving_judgment_threshold: 2.0

    velocity_scale_factor:
      estimated_minimum_interval: 20
      estimated_maximum_interval: 400
      gnss_receiving_threshold: 0.25
      save_velocity_scale_factor: false
      velocity_scale_factor_save_duration: 100.0
      th_velocity_scale_factor_percent: 10.0

    yaw_rate_offset_stop:
      estimated_interval: 3
      outlier_threshold: 0.002

    yaw_rate_offset:
      estimated_minimum_interval: 30
      gnss_receiving_threshold: 0.25
      outlier_threshold: 0.002
      1st:
        estimated_maximum_interval: 300
      2nd:
        estimated_maximum_interval: 500

    heading:
      estimated_minimum_interval: 10
      estimated_maximum_interval: 30
      gnss_receiving_threshold: 0.25
      outlier_threshold: 0.0524
      outlier_ratio_threshold: 0.5
      curve_judgment_threshold: 0.0873
      init_STD: 0.0035 #[rad] (= 0.2 [deg])

    heading_interpolate:
      sync_search_period: 2
      proc_noise: 0.0005 #[rad] (= 0.03 [deg])

    slip_angle:
      manual_coefficient: 0.0

    slip_coefficient:
      estimated_minimum_interval: 2
      estimated_maximum_interval: 100
      curve_judgment_threshold: 0.017453
      lever_arm: 0.0

    rolling:
      filter_process_noise: 0.01
      filter_observation_noise: 1

    trajectory:
      curve_judgment_threshold: 0.017453
      timer_update_rate: 10
      deadlock_threshold: 1
      sensor_noise_velocity: 0.05
      sensor_scale_noise_velocity: 0.02
      sensor_noise_yaw_rate: 0.01
      sensor_bias_noise_yaw_rate: 0.1

    smoothing:
      moving_average_time: 3
      moving_ratio_threshold: 0.1

    height:
      estimated_minimum_interval: 200
      estimated_maximum_interval: 2000
      update_distance: 0.1
      gnss_receiving_threshold: 0.1
      outlier_threshold: 0.3
      outlier_ratio_threshold: 0.5
      moving_average_time: 1

    position:
      estimated_interval: 300
      update_distance: 0.1
      outlier_threshold: 3.0
      gnss_receiving_threshold: 0.25
      outlier_ratio_threshold: 0.5

    position_interpolate:
      sync_search_period: 2

    # Optional Navigation Functions
    angular_velocity_offset_stop:
      estimated_interval: 4
      outlier_threshold: 0.002

    rtk_dead_reckoning:
      rtk_fix_STD: 0.3 #[m]
      proc_noise: 0.05 #[m]

    rtk_heading:
      update_distance: 0.3
      estimated_minimum_interval: 10
      estimated_maximum_interval: 30
      gnss_receiving_threshold: 0.25
      outlier_threshold: 0.0524
      outlier_ratio_threshold: 0.5
      curve_judgment_threshold: 0.0873

    enable_additional_rolling:
      update_distance: 0.3
      moving_average_time: 1
      sync_judgment_threshold: 0.01
      sync_search_period: 1

    velocity_estimator:
      gga_downsample_time: 0.5
      stop_judgment_velocity_threshold: 0.2
      stop_judgment_interval: 1
      variance_threshold: 0.000025
      pitch_rate_offset:
        estimated_interval: 8
      pitching:
        estimated_interval: 5
        outlier_threshold: 0.0174
        gnss_receiving_threshold: 0.2
        outlier_ratio_threshold: 0.5
      acceleration_offset:
        estimated_minimum_interval: 30
        estimated_maximum_interval: 500
        filter_process_noise: 0.01
        filter_observation_noise: 1
      doppler_fusion:
        estimated_interval: 4
        gnss_receiving_threshold: 0.2
        outlier_ratio_threshold: 0.5
        outlier_threshold: 0.1

And here is the launch file

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from base_common.launch import create_node_name_with_launch_var, get_sim_time_launch_arg, get_share_file

## TODO(github/sisaha9): Look into replacing all the namespaces with PushRosNamespace

def generate_launch_description():
    declare_use_sim_time_cmd, use_sim_time = get_sim_time_launch_arg()
    eagleye_config_fp = DeclareLaunchArgument("eagleye_config_fp", default_value="None", description="Path to the eagleye config file to use")
    unique_identifier = DeclareLaunchArgument("unique_identifier", default_value="None", description="Unique identifier to use")
    
    twist_relay_node = Node(
        package="eagleye_rt",
        executable="twist_relay",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_twist_relay",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time
        ],
    )
    
    tf_converted_imu_node = Node(
        package="eagleye_rt",
        executable="tf_converted_imu",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_tf_converted_imu",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time
        ]
    )
    
    velocity_scale_factor_node = Node(
        package="eagleye_rt",
        executable="velocity_scale_factor",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_velocity_scale_factor",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            { "velocity_scale_factor_save_str": (
                get_share_file("autoware_localization_launch", "config"),
                "/",
                LaunchConfiguration("unique_identifier"),
                "/velocity_scale_factor.txt",
            ),
            "yaml_file" : LaunchConfiguration("eagleye_config_fp")
            }
        ]
    )
    
    yaw_rate_offset_stop_node = Node(
        package="eagleye_rt",
        executable="yaw_rate_offset_stop",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_yaw_rate_offset_stop",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    yaw_rate_offset_node_1st = Node(
        package="eagleye_rt",
        executable="yaw_rate_offset",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_yaw_rate_offset_1st",
        arguments=["1st"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    yaw_rate_offset_node_2nd = Node(
        package="eagleye_rt",
        executable="yaw_rate_offset",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_yaw_rate_offset_2nd",
        arguments=["2nd"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    heading_node_1st = Node(
        package="eagleye_rt",
        executable="heading",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_heading_1st",
        arguments=["1st"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    heading_node_2nd = Node(
        package="eagleye_rt",
        executable="heading",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_heading_2nd",
        arguments=["2nd"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]   
    )
    
    heading_node_3rd = Node(
        package="eagleye_rt",
        executable="heading",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_heading_3rd",
        arguments=["3rd"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]   
    )
    
    heading_interpolate_node_1st = Node(
        package="eagleye_rt",
        executable="heading_interpolate",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_heading_interpolate_1st",
        arguments=["1st"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    heading_interpolate_node_2nd = Node(
        package="eagleye_rt",
        executable="heading_interpolate",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_heading_interpolate_2nd",
        arguments=["2nd"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    heading_interpolate_node_3rd = Node(
        package="eagleye_rt",
        executable="heading_interpolate",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_heading_interpolate_3rd",
        arguments=["3rd"],
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    slip_angle_node = Node(
        package="eagleye_rt",
        executable="slip_angle",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_slip_angle",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")},
        ]
    )
    
    distance_node = Node(
        package="eagleye_rt",
        executable="distance",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_distance",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time
        ]
    )
    
    trajectory_node = Node(
        package="eagleye_rt",
        executable="trajectory",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_trajectory",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")},
        ]
    )
    
    position_node = Node(
        package="eagleye_rt",
        executable="position",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_position",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")},
        ]
    )
    
    position_interpolate_node = Node(
        package="eagleye_rt",
        executable="position_interpolate",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_position_interpolate",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")},
        ]
    )
    
    smoothing_node = Node(
        package="eagleye_rt",
        executable="smoothing",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_smoothing",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")},
        ]
    )
    
    height_node = Node(
        package="eagleye_rt",
        executable="height",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_height",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")},
        ]
    )
    
    angular_velocity_offset_stop_node = Node(
        package="eagleye_rt",
        executable="angular_velocity_offset_stop",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_angular_velocity_offset_stop",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")},
        ]
    )
    
    correction_imu_node = Node(
        package="eagleye_rt",
        executable="correction_imu",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_correction_imu",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time
        ]
    )
    
    rolling_node = Node(
        package="eagleye_rt",
        executable="rolling",
        namespace=LaunchConfiguration("unique_identifier"),
        name="eagleye_rolling",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time,
            {"yaml_file" : LaunchConfiguration("eagleye_config_fp")}
        ]
    )
    
    gnss_converter_node = Node(
        package="eagleye_gnss_converter",
        executable="gnss_converter",
        namespace=(
            LaunchConfiguration("unique_identifier"),
            "/gnss"
        ),
        name="eagleye_gnss_converter",
        parameters=[
            LaunchConfiguration("eagleye_config_fp"),
            use_sim_time
        ]
    )
    
    return LaunchDescription([
        declare_use_sim_time_cmd,
        eagleye_config_fp,
        unique_identifier,
        twist_relay_node,
        tf_converted_imu_node,
        velocity_scale_factor_node,
        yaw_rate_offset_stop_node,
        yaw_rate_offset_node_1st,
        yaw_rate_offset_node_2nd,
        heading_node_1st,
        heading_node_2nd,
        heading_node_3rd,
        heading_interpolate_node_1st,
        heading_interpolate_node_2nd,
        heading_interpolate_node_3rd,
        slip_angle_node,
        distance_node,
        trajectory_node,
        position_node,
        position_interpolate_node,
        smoothing_node,
        height_node,
        angular_velocity_offset_stop_node,
        correction_imu_node,
        rolling_node,
        gnss_converter_node
    ])

[sisaha9]

Not sure if this should be in a separate issue but it doesn't look like the RPY get's updated either as seen in the pose on the top left panel that comes in the screen a couple of times

[rsasaki0109]

Is it possible to have us provide rosbag?

[sisaha9]

Hi @rsasaki0109 . Here is a link: https://drive.google.com/drive/folders/1nmsM7Fz6pjkn_Nj_x4qW8Ee0N8TxVvRi?usp=sharing . It is right now shared with the public email on your Github profile

[rsasaki0109]

Thank you. I'm busy during the weekdays this week so I'll look at it in my free time.