Position seems to be offsetted in Eagleye. What could be causing this?
sisaha9 opened this issue · comments
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.1And 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
])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
Is it possible to have us provide rosbag?
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
Thank you. I'm busy during the weekdays this week so I'll look at it in my free time.