diff --git a/CMakeLists.txt b/CMakeLists.txt index bb7dc43..c21a547 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,7 +35,6 @@ set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) set(LIBRARIES MrsUavStateEstimators_Common - MrsUavStateEstimators_Aloam MrsUavStateEstimators_GpsGarmin MrsUavStateEstimators_GpsBaro MrsUavStateEstimators_Rtk @@ -134,23 +133,6 @@ add_dependencies(MrsUavStateEstimators_GpsBaro ${PROJECT_NAME}_gencfg ) -add_library(MrsUavStateEstimators_Aloam - src/estimators/state/aloam.cpp - ) - -target_link_libraries(MrsUavStateEstimators_Aloam - MrsUavStateEstimators_Common - ${catkin_LIBRARIES} - ${Eigen_LIBRARIES} - ) - -## Add configure headers for dynamic reconfigure -add_dependencies(MrsUavStateEstimators_Aloam - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg - ) - add_library(MrsUavStateEstimators_HectorGarmin src/estimators/state/hector_garmin.cpp ) diff --git a/config/private/aloam/aloam.yaml b/config/private/aloam/aloam.yaml deleted file mode 100644 index f3c1573..0000000 --- a/config/private/aloam/aloam.yaml +++ /dev/null @@ -1,26 +0,0 @@ -mrs_uav_managers: - estimation_manager: - aloam: # namespace of the state estimator - requires: # data required from the hw api - gnss: false - imu: false - distance_sensor: false - altitude: false - magnetometer_heading: false - position: false - orientation: true - velocity: true - angular_velocity: true - - estimators: # the names of the partial estimators - lateral: - name: "lat_aloam" - altitude: - name: "alt_aloam" - heading: - name: "hdg_aloam" - passthrough: false # if true, then heading is not estimated but passed through from the orientation topic - - topics: - orientation: "hw_api/orientation" # orientation passthrough - angular_velocity: "hw_api/angular_velocity" # angular velocity passthrough diff --git a/config/private/aloam/alt_aloam.yaml b/config/private/aloam/alt_aloam.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/aloam/hdg_aloam.yaml b/config/private/aloam/hdg_aloam.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/aloam/lat_aloam.yaml b/config/private/aloam/lat_aloam.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/public/aloam/aloam.yaml b/config/public/aloam/aloam.yaml deleted file mode 100644 index 026678e..0000000 --- a/config/public/aloam/aloam.yaml +++ /dev/null @@ -1,6 +0,0 @@ -mrs_uav_managers: - estimation_manager: - aloam: # namespace of the state estimator - override_frame_id: # override the default frame_id with a custom one (e.g. slam_origin instead of aloam_origin) - enabled: true - frame_id: "slam_origin" # the desired frame_id without the UAV namespace diff --git a/config/public/aloam/alt_aloam.yaml b/config/public/aloam/alt_aloam.yaml deleted file mode 100644 index 799abe0..0000000 --- a/config/public/aloam/alt_aloam.yaml +++ /dev/null @@ -1,55 +0,0 @@ -mrs_uav_managers: - estimation_manager: - aloam: # namespace of the state estimator - alt_aloam: # namespace of the altitude estimator - - max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame) - - innovation: - limit: 1.0 # [m] innovation limit that will trigger action - action: "eland" # {"eland", "switch", "mitigate"} - - repredictor: # repredictor for correct fusion of delayed measurements - enabled: true - buffer_size: 200 # [samples] how many states and corrections are kept in history (i.e. estimator running at the default 100 Hz rate will be able to fuse corrections with up to 2 s delay with buffer size of 200 samples) - - process_noise: # process noise covariance (Q) - pos: 1.0 # position state - vel: 100.0 # velocity state - acc: 100.0 # acceleration state - - corrections: [ - "pos_aloam", - "vel_hw_api" - ] - - pos_aloam: - state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.1 # measurement noise covariance (R) - noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) - message: - type: "nav_msgs/Odometry" - topic: "slam/odom" # without uav namespace - limit: - delay: 2.0 # [s] messages with higher delay will flag correction as unhealthy - time_since_last: 1.0 # [s] larger time step between messages will flag correction as unhealthy - - processors: [] # types of processors attached to this measurement - - vel_hw_api: - state_id: 1 # 0 - position, 1 - velocity, 2 - acceleration - noise: 1.0 # measurement noise covariance (R) - noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) - message: - type: "geometry_msgs/Vector3Stamped" - topic: "hw_api/velocity" # without uav namespace - limit: - delay: 1.0 # [s] messages with higher delay will flag correction as unhealthy - time_since_last: 0.5 # [s] larger time step between messages will flag correction as unhealthy - - processors: [] # types of processors attached to this measurement - - median_filter: - buffer_size: 20 # the number of samples from which the median is calculated - max_diff: 2.0 # [m] reject measurement if difference from median is over this value - diff --git a/config/public/aloam/hdg_aloam.yaml b/config/public/aloam/hdg_aloam.yaml deleted file mode 100644 index 95f611d..0000000 --- a/config/public/aloam/hdg_aloam.yaml +++ /dev/null @@ -1,48 +0,0 @@ -mrs_uav_managers: - estimation_manager: - aloam: # namespace of the state estimator - hdg_aloam: # namespace of the heading estimator - - max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame) - - position_innovation_limit: 1.0 # [m] innovation limit that will trigger switch to other estimator - - repredictor: # repredictor for correct fusion of delayed measurements - enabled: true - buffer_size: 200 # [samples] how many states and corrections are kept in history (i.e. estimator running at the default 100 Hz rate will be able to fuse corrections with up to 2 s delay with buffer size of 200 samples) - - process_noise: # process noise covariance (Q) - pos: 0.1 # position state - vel: 0.1 # velocity state - - corrections: [ - "hdg_aloam", - "hdg_rate_gyro" - ] - - hdg_aloam: - state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.01 # measurement noise covariance (R) - noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) - message: - type: "nav_msgs/Odometry" - topic: "slam/odom" # without uav namespace - limit: - delay: 2.0 # [s] messages with higher delay will flag correction as unhealthy - time_since_last: 1.0 # [s] larger time step between messages will flag correction as unhealthy - - processors: [] # types of processors attached to this measurement - - hdg_rate_gyro: - state_id: 1 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.1 # measurement noise covariance (R) - noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) - message: - type: "geometry_msgs/Vector3Stamped" - topic: "hw_api/angular_velocity" # without uav namespace - orientation_topic: "hw_api/orientation" # without uav namespace - limit: - delay: 2.0 # [s] messages with higher delay will flag correction as unhealthy - time_since_last: 1.0 # [s] larger time step between messages will flag correction as unhealthy - - processors: [] # types of processors attached to this measurement diff --git a/config/public/aloam/lat_aloam.yaml b/config/public/aloam/lat_aloam.yaml deleted file mode 100644 index ed39a9d..0000000 --- a/config/public/aloam/lat_aloam.yaml +++ /dev/null @@ -1,39 +0,0 @@ -mrs_uav_managers: - estimation_manager: - aloam: # namespace of the state estimator - lat_aloam: # namespace of the lateral estimator - - hdg_source_topic: "aloam/hdg_aloam/output" # [mrs_uav_state_estimation/EstimatorOutput] - - innovation: - limit: 1.0 # [m] innovation limit that will trigger action - action: "eland" # {"eland", "switch", "mitigate"} - - max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame) - - repredictor: # repredictor for correct fusion of delayed measurements - enabled: true - buffer_size: 200 # [samples] how many states and corrections are kept in history (i.e. estimator running at the default 100 Hz rate will be able to fuse corrections with up to 2 s delay with buffer size of 200 samples) - - process_noise: # process noise covariance (Q) - pos: 1.0 # position state - vel: 10.0 # velocity state - acc: 0.1 # acceleration state - - corrections: [ - "pos_aloam" - ] - - pos_aloam: - state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.001 # measurement noise covariance (R) - noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) - message: - type: "nav_msgs/Odometry" - topic: "slam/odom" # without uav namespace - limit: - delay: 2.0 # [s] messages with higher delay will flag correction as unhealthy - time_since_last: 1.0 # [s] larger time step between messages will flag correction as unhealthy - - processors: [] # types of processors attached to this measurement - diff --git a/estimator_plugins.xml b/estimator_plugins.xml index d0221f7..e2a7a34 100644 --- a/estimator_plugins.xml +++ b/estimator_plugins.xml @@ -22,12 +22,6 @@ - - - Aloam state estimator - - - HectorGarmin state estimator diff --git a/src/estimators/state/aloam.cpp b/src/estimators/state/aloam.cpp deleted file mode 100644 index 8f52750..0000000 --- a/src/estimators/state/aloam.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - - -namespace mrs_uav_state_estimators -{ - -namespace aloam -{ - -const char estimator_name[] = "aloam"; -const bool is_core_plugin = true; - -class Aloam : public StateGeneric { -public: - Aloam() : StateGeneric(estimator_name, is_core_plugin) { - } - - ~Aloam(void) { - } -}; - -} // namespace aloam -} // namespace mrs_uav_state_estimators - -#include -PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::aloam::Aloam, mrs_uav_managers::StateEstimator)