diff --git a/src/state_estimation/state_estimation/launch/ekf.launch.py b/src/state_estimation/state_estimation/launch/ekf.launch.py index 8c99b88b..993fdabc 100644 --- a/src/state_estimation/state_estimation/launch/ekf.launch.py +++ b/src/state_estimation/state_estimation/launch/ekf.launch.py @@ -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' @@ -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', @@ -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 diff --git a/src/state_estimation/state_estimation/param/ekf_select.yaml b/src/state_estimation/state_estimation/param/ekf_select.yaml index 2f452d43..58cba241 100644 --- a/src/state_estimation/state_estimation/param/ekf_select.yaml +++ b/src/state_estimation/state_estimation/param/ekf_select.yaml @@ -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] \ No newline at end of file + 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