diff --git a/CMakeLists.txt b/CMakeLists.txt index c21a547..ad20a59 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,8 +39,6 @@ set(LIBRARIES MrsUavStateEstimators_GpsBaro MrsUavStateEstimators_Rtk MrsUavStateEstimators_RtkGarmin - MrsUavStateEstimators_HectorGarmin - MrsUavStateEstimators_Vio MrsUavStateEstimators_Passthrough MrsUavStateEstimators_GroundTruth MrsUavStateEstimators_Dummy @@ -133,40 +131,6 @@ add_dependencies(MrsUavStateEstimators_GpsBaro ${PROJECT_NAME}_gencfg ) -add_library(MrsUavStateEstimators_HectorGarmin - src/estimators/state/hector_garmin.cpp - ) - -target_link_libraries(MrsUavStateEstimators_HectorGarmin - MrsUavStateEstimators_Common - ${catkin_LIBRARIES} - ${Eigen_LIBRARIES} - ) - -## Add configure headers for dynamic reconfigure -add_dependencies(MrsUavStateEstimators_HectorGarmin - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg - ) - -add_library(MrsUavStateEstimators_Vio - src/estimators/state/vio.cpp - ) - -target_link_libraries(MrsUavStateEstimators_Vio - MrsUavStateEstimators_Common - ${catkin_LIBRARIES} - ${Eigen_LIBRARIES} - ) - -## Add configure headers for dynamic reconfigure -add_dependencies(MrsUavStateEstimators_Vio - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg - ) - add_library(MrsUavStateEstimators_Rtk src/estimators/state/rtk.cpp ) diff --git a/config/private/hector_garmin/alt_garmin.yaml b/config/private/hector_garmin/alt_garmin.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/hector_garmin/hdg_hector.yaml b/config/private/hector_garmin/hdg_hector.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/hector_garmin/hector_garmin.yaml b/config/private/hector_garmin/hector_garmin.yaml deleted file mode 100644 index 580a864..0000000 --- a/config/private/hector_garmin/hector_garmin.yaml +++ /dev/null @@ -1,26 +0,0 @@ -mrs_uav_managers: - estimation_manager: - hector_garmin: # namespace of the state estimator - requires: # data required from the hw api - gnss: false - imu: false - distance_sensor: true - altitude: false - magnetometer_heading: false - position: false - orientation: true - velocity: true - angular_velocity: true - - estimators: # the names of the partial estimators - lateral: - name: "lat_hector" - altitude: - name: "alt_garmin" - heading: - name: "hdg_hector" - 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/hector_garmin/lat_hector.yaml b/config/private/hector_garmin/lat_hector.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/vio/alt_garmin.yaml b/config/private/vio/alt_garmin.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/vio/alt_vio.yaml b/config/private/vio/alt_vio.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/vio/hdg_vio.yaml b/config/private/vio/hdg_vio.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/vio/lat_vio.yaml b/config/private/vio/lat_vio.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/private/vio/vio.yaml b/config/private/vio/vio.yaml deleted file mode 100644 index 2588edd..0000000 --- a/config/private/vio/vio.yaml +++ /dev/null @@ -1,26 +0,0 @@ -mrs_uav_managers: - estimation_manager: - vio: # 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_vio" - altitude: - name: "alt_vio" - heading: - name: "hdg_vio" - 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/public/hector_garmin/alt_garmin.yaml b/config/public/hector_garmin/alt_garmin.yaml deleted file mode 100644 index d35bb59..0000000 --- a/config/public/hector_garmin/alt_garmin.yaml +++ /dev/null @@ -1,66 +0,0 @@ -mrs_uav_managers: - estimation_manager: - hector_garmin: # namespace of the state estimator - alt_garmin: # namespace of the altitude estimator - - max_flight_z: 10.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 - - 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: false - - process_noise: # process noise covariance (Q) - pos: 1.0 # position state - vel: 1.0 # velocity state - acc: 1.0 # acceleration state - - corrections: [ - "pos_garmin", - "vel_hw_api" - ] - - pos_garmin: - state_id: 0 # 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: "sensor_msgs/Range" - topic: "hw_api/distance_sensor" # without uav namespace - limit: - delay: 0.5 # [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: ["excessive_tilt", "median_filter", "saturate"] # types of processors attached to this measurement (the processors are called in exactly this order) - - median_filter: - buffer_size: 100 # [samples] number of historic values from which the median is calculated - max_diff: 2.0 # [m] difference of input value from median to be considered valid - - saturate: - start_enabled: true # saturate processor can start as disabled and then be enabled by a service or a trigger condition - keep_enabled: true # keep enabled even after the corrections are near the value of state - min: -0.1 - max: 0.1 - - excessive_tilt: - orientation_topic: "hw_api/orientation" - max_tilt: 30.0 # [deg] - - # Parameters of altitude median filters - buffer_size , max_diff - 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 diff --git a/config/public/hector_garmin/hdg_hector.yaml b/config/public/hector_garmin/hdg_hector.yaml deleted file mode 100644 index 28bf2f2..0000000 --- a/config/public/hector_garmin/hdg_hector.yaml +++ /dev/null @@ -1,33 +0,0 @@ -mrs_uav_managers: - estimation_manager: - hector_garmin: # namespace of the state estimator - hdg_hector: # namespace of the heading estimator - - max_flight_z: 10.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_hector" - ] - - hdg_hector: - state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.0001 # measurement noise covariance (R) - noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) - message: - type: "geometry_msgs/PoseStamped" - topic: "hector_mapping/slam_out_pose" # 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/hector_garmin/hector_garmin.yaml b/config/public/hector_garmin/hector_garmin.yaml deleted file mode 100644 index 5d9b0e8..0000000 --- a/config/public/hector_garmin/hector_garmin.yaml +++ /dev/null @@ -1,11 +0,0 @@ -mrs_uav_managers: - estimation_manager: - hector_garmin: # 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: false - frame_id: "hector_origin" # the desired frame_id without the UAV namespace - - - - - diff --git a/config/public/hector_garmin/lat_hector.yaml b/config/public/hector_garmin/lat_hector.yaml deleted file mode 100644 index 35ed8ea..0000000 --- a/config/public/hector_garmin/lat_hector.yaml +++ /dev/null @@ -1,39 +0,0 @@ -mrs_uav_managers: - estimation_manager: - hector_garmin: # namespace of the state estimator - lat_hector: # namespace of the lateral estimator - - hdg_source_topic: "hector_garmin/hdg_hector/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: 10.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: 0.01 # position state - vel: 0.1 # velocity state - acc: 0.1 # acceleration state - - corrections: [ - "pos_hector" - ] - - pos_hector: - 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: "geometry_msgs/PoseStamped" - topic: "hector_mapping/slam_out_pose" # 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/vio/alt_garmin.yaml b/config/public/vio/alt_garmin.yaml deleted file mode 100644 index 1182c45..0000000 --- a/config/public/vio/alt_garmin.yaml +++ /dev/null @@ -1,65 +0,0 @@ -mrs_uav_managers: - estimation_manager: - vio: # namespace of the state estimator - alt_garmin: # namespace of the altitude estimator - - max_flight_z: 10.0 # [m] maximum allowed flight Z (in the estimator frame) - - innovation: - limit: 1.0 # [m] innovation limit that will trigger action - action: "mitigate" # {"eland", "switch", "mitigate"} - - repredictor: # repredictor for correct fusion of delayed measurements - enabled: false - - process_noise: # process noise covariance (Q) - pos: 1.0 # position state - vel: 1.0 # velocity state - acc: 1.0 # acceleration state - - corrections: [ - "pos_garmin", - "vel_hw_api" - ] - - pos_garmin: - state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 10.0 # measurement noise covariance (R) - noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) - message: - type: "sensor_msgs/Range" - topic: "hw_api/distance_sensor" # without uav namespace - limit: - delay: 0.5 # [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: ["excessive_tilt", "median_filter", "saturate"] # types of processors attached to this measurement (the processors are called in exactly this order) - - median_filter: - buffer_size: 100 # [samples] number of historic values from which the median is calculated - max_diff: 2.0 # [m] difference of input value from median to be considered valid - - saturate: - start_enabled: true # saturate processor can start as disabled and then be enabled by a service or a trigger condition - keep_enabled: true # keep enabled even after the corrections are near the value of state - min: -0.1 - max: 0.1 - limit: 2.0 - - excessive_tilt: - orientation_topic: "hw_api/orientation" - max_tilt: 30.0 # [deg] - - # Parameters of altitude median filters - buffer_size , max_diff - vel_hw_api: - state_id: 1 # 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: "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 diff --git a/config/public/vio/alt_vio.yaml b/config/public/vio/alt_vio.yaml deleted file mode 100644 index 2d4fa60..0000000 --- a/config/public/vio/alt_vio.yaml +++ /dev/null @@ -1,55 +0,0 @@ -mrs_uav_managers: - estimation_manager: - vio: # namespace of the state estimator - alt_vio: # 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: false - 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_vio", - "vel_hw_api" - ] - - pos_vio: - 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: "vins_republisher/odom" # 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 - - 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/vio/hdg_vio.yaml b/config/public/vio/hdg_vio.yaml deleted file mode 100644 index 3de2b81..0000000 --- a/config/public/vio/hdg_vio.yaml +++ /dev/null @@ -1,33 +0,0 @@ -mrs_uav_managers: - estimation_manager: - vio: # namespace of the state estimator - hdg_vio: # namespace of the heading estimator - - max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame) - - position_innovation_limit: 1.0 # [rad] 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_vio" - ] - - hdg_vio: - state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.0001 # 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: "vins_republisher/odom" # 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 diff --git a/config/public/vio/lat_vio.yaml b/config/public/vio/lat_vio.yaml deleted file mode 100644 index e17732d..0000000 --- a/config/public/vio/lat_vio.yaml +++ /dev/null @@ -1,53 +0,0 @@ -mrs_uav_managers: - estimation_manager: - vio: # namespace of the state estimator - lat_vio: # namespace of the lateral estimator - - hdg_source_topic: "vio/hdg_vio/output" # [mrs_uav_state_estimation/EstimatorOutput] - - 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: 0.1 # position state - vel: 1.0 # velocity state - acc: 1.0 # acceleration state - - corrections: [ - "pos_vio", - "vel_vio" - ] - - pos_vio: - 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: "vins_republisher/odom" # 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 - - vel_vio: - state_id: 1 # 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) - body_frame: false # velocity is in global frame from vins-mono - message: - type: "nav_msgs/Odometry" - topic: "vins_republisher/odom" # 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 diff --git a/config/public/vio/vio.yaml b/config/public/vio/vio.yaml deleted file mode 100644 index a01512d..0000000 --- a/config/public/vio/vio.yaml +++ /dev/null @@ -1,6 +0,0 @@ -mrs_uav_managers: - estimation_manager: - vio: # namespace of the state estimator - override_frame_id: # override the default frame_id with a custom one (e.g. slam_origin instead of vio_origin) - enabled: false - frame_id: "vio_origin" # the desired frame_id without the UAV namespace diff --git a/estimator_plugins.xml b/estimator_plugins.xml index e2a7a34..d1874c8 100644 --- a/estimator_plugins.xml +++ b/estimator_plugins.xml @@ -22,18 +22,6 @@ - - - HectorGarmin state estimator - - - - - - Vio state estimator - - - Passthrough state estimator diff --git a/src/estimators/state/hector_garmin.cpp b/src/estimators/state/hector_garmin.cpp deleted file mode 100644 index 58a5f90..0000000 --- a/src/estimators/state/hector_garmin.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include - -namespace mrs_uav_state_estimators -{ - -namespace hector_garmin -{ - -const char estimator_name[] = "hector_garmin"; -const bool is_core_plugin = true; - -class HectorGarmin : public StateGeneric { -public: - HectorGarmin() : StateGeneric(estimator_name, is_core_plugin) { - } - - ~HectorGarmin(void) { - } -}; - -} // namespace hector_garmin -} // namespace mrs_uav_state_estimators - -#include -PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::hector_garmin::HectorGarmin, mrs_uav_managers::StateEstimator) diff --git a/src/estimators/state/vio.cpp b/src/estimators/state/vio.cpp deleted file mode 100644 index c2ea9c5..0000000 --- a/src/estimators/state/vio.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include - -namespace mrs_uav_state_estimators -{ - -namespace vio -{ - -const char estimator_name[] = "vio"; -const bool is_core_plugin = true; - -class Vio : public StateGeneric { -public: - Vio() : StateGeneric(estimator_name, is_core_plugin) { - } - - ~Vio(void) { - } -}; - -} // namespace vio -} // namespace mrs_uav_state_estimators - -#include -PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::vio::Vio, mrs_uav_managers::StateEstimator)