Skip to content

Commit

Permalink
WIP: state_estimation pkg, able to manually select the sensor sourc…
Browse files Browse the repository at this point in the history 
…es we want to do sensor fusion by just turning on or off in `ekf_select.yaml`.
  • Loading branch information
@WinstonHChou
WinstonHChou committed May 26, 2024
1 parent ee0a588 commit e8b93fb
Show file tree
Hide file tree
Showing 2 changed files with 110 additions and 6 deletions.
92 changes: 89 additions & 3 deletions src/state_estimation/state_estimation/launch/ekf.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,90 @@ def read_yaml(yaml_input_path):
yaml_inputs = yaml.load(file, Loader=yaml.SafeLoader)
return yaml_inputs

def parse_select(select_file_path: str, kalman_filter_type: str):
"""
parse_select(select_file_path, kalman_filter_type)
- The function helps parse the select file
for `ekf_node` or `ukf_node` in `robot_localization` pkg.
- Please name your select yaml file as `ekf_select.yaml` or `ukf_select.yaml`.
- Specify `ekf_config` or `ukf_config` in the 1st line of your yaml file.
"""
select_prefix = kalman_filter_type + '_select'
override_prefix = 'override_' + kalman_filter_type + '_config'
select_yaml = read_yaml(select_file_path)
arguments_ = []
override_params_ = dict()
if select_yaml[select_prefix][override_prefix]:
num_odoms = 0
num_imus = 0
odom_names = []
imu_names = []
for odom_arg, odom_dict in select_yaml[select_prefix]['odom'].items():
if odom_dict['enable'] == 1:
odom_arg_num = 'odom' + str(num_odoms)
arguments_.append(DeclareLaunchArgument(str(odom_arg),
default_value=odom_dict['odom_topic']))
override_params_[odom_arg_num] = LaunchConfiguration(str(odom_arg))

odom_arg_config = odom_arg_num + '_config'
arguments_.append(DeclareLaunchArgument(str(odom_arg + '_config'),
default_value=
str(odom_dict['odom_config'])))
override_params_[odom_arg_config] = LaunchConfiguration(str(odom_arg + '_config'))

odom_arg_queue_size = odom_arg_num + '_queue_size'
arguments_.append(DeclareLaunchArgument(str(odom_arg + '_queue_size'),
default_value=
str(odom_dict['odom_queue_size'])))
override_params_[odom_arg_queue_size] = LaunchConfiguration(str(odom_arg + '_queue_size'))

odom_arg_rel = odom_arg_num + '_relative'
arguments_.append(DeclareLaunchArgument(str(odom_arg + '_relative'),
default_value=
str(odom_dict['odom_relative'])))
override_params_[odom_arg_rel] = LaunchConfiguration(str(odom_arg + '_relative'))

num_odoms += 1
odom_names.append(str(odom_arg))

for imu_arg, imu_dict in select_yaml[select_prefix]['imu'].items():
if imu_dict['enable'] == 1:
imu_arg_num = 'imu' + str(num_imus)
arguments_.append(DeclareLaunchArgument(str(imu_arg),
default_value=imu_dict['imu_topic']))
override_params_[imu_arg_num] = LaunchConfiguration(str(imu_arg))

imu_arg_config = imu_arg_num + '_config'
arguments_.append(DeclareLaunchArgument(str(imu_arg + '_config'),
default_value=
str(imu_dict['imu_config'])))
override_params_[imu_arg_config] = LaunchConfiguration(str(imu_arg + '_config'))

imu_arg_queue_size = imu_arg_num + '_queue_size'
arguments_.append(DeclareLaunchArgument(str(imu_arg + '_queue_size'),
default_value=
str(imu_dict['imu_queue_size'])))
override_params_[imu_arg_queue_size] = LaunchConfiguration(str(imu_arg + '_queue_size'))

imu_arg_rel = imu_arg_num + '_relative'
arguments_.append(DeclareLaunchArgument(str(imu_arg + '_relative'),
default_value=
str(imu_dict['imu_relative'])))
override_params_[imu_arg_rel] = LaunchConfiguration(str(imu_arg + '_relative'))

imu_arg_gravity = imu_arg_num + '_remove_gravitational_acceleration'
arguments_.append(DeclareLaunchArgument(str(imu_arg + '_gravity'),
default_value=
str(imu_dict['remove_gravitational_acceleration'])))
override_params_[imu_arg_gravity] = LaunchConfiguration(str(imu_arg + '_gravity'))

num_imus += 1
imu_names.append(str(imu_arg))

print('[Sensor Fusion]', 'Num of Odom:', num_odoms, ', Names of Odom:', odom_names)
print('[Sensor Fusion]', 'Num of IMU:', num_imus, ', Names of IMU:', imu_names)
return arguments_, override_params_

def generate_launch_description():
pkg_name = 'state_estimation'
config_file = 'ekf_config.yaml'
Expand All @@ -31,8 +115,7 @@ def generate_launch_description():
select_file
)

select_yaml = read_yaml(select_file_path)
# if select_yaml['ekf_select']['override_ekf_config'] ==
ekf_arguments, override_params = parse_select(select_file_path, 'ekf')

robot_localization_node = Node(
package='robot_localization',
Expand All @@ -41,11 +124,14 @@ def generate_launch_description():
output='screen',
parameters=[
config_file_path,
{'use_sim_time': False}
{'use_sim_time': False},
override_params
]
)

ld = LaunchDescription()
for arg in ekf_arguments:
ld.add_entity(arg)
ld.add_action(robot_localization_node)

return ld
24 changes: 21 additions & 3 deletions src/state_estimation/state_estimation/param/ekf_select.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,35 @@ ekf_select: # select which source to disable (0) or enable (1)
odom:
vesc_odom:
enable: 1
odom_topic: "/odom"
odom_topic: /odom
odom_config: [true, true, false, # [x_pos , y_pos , z_pos,
false, false, true, # roll , pitch , yaw,
false, false, false, # x_vel , y_vel , z_vel,
false, false, true, # roll_vel, pitch_vel, yaw_vel,
false, false, false] # x_accel , y_accel , z_accel]
odom_queue_size: 1
odom_relative: true

rf2o:
enable: 0
odom_topic: "/rf2o/odom"
odom_topic: /rf2o/odom
odom_config: [true, true, false, # [x_pos , y_pos , z_pos,
false, false, true, # roll , pitch , yaw,
false, false, false, # x_vel , y_vel , z_vel,
false, false, true, # roll_vel, pitch_vel, yaw_vel,
false, false, false] # x_accel , y_accel , z_accel]
false, false, false] # x_accel , y_accel , z_accel]
odom_queue_size: 1
odom_relative: true

imu:
livox_imu:
enable: 1
imu_topic: /lidar/imu
imu_config: [false, false, false, # [x_pos , y_pos , z_pos,
true, true, true, # roll , pitch , yaw,
false, false, false, # x_vel , y_vel , z_vel,
false, false, false, # roll_vel, pitch_vel, yaw_vel,
false, false, false] # x_accel , y_accel , z_accel]
imu_queue_size: 5
imu_relative: true
remove_gravitational_acceleration: false

0 comments on commit e8b93fb

Please sign in to comment.