added rtk_garmin estimator

CMakeLists.txt

@@ -39,6 +39,7 @@ set(LIBRARIES
   MrsUavStateEstimators_GpsGarmin
   MrsUavStateEstimators_GpsBaro
   MrsUavStateEstimators_Rtk
+  MrsUavStateEstimators_RtkGarmin
   MrsUavStateEstimators_HectorGarmin
   MrsUavStateEstimators_Vio
   MrsUavStateEstimators_Passthrough
@@ -201,6 +202,23 @@ add_dependencies(MrsUavStateEstimators_Rtk
   ${PROJECT_NAME}_gencfg
   )
 
+add_library(MrsUavStateEstimators_RtkGarmin
+  src/estimators/state/rtk_garmin.cpp
+  )
+
+target_link_libraries(MrsUavStateEstimators_RtkGarmin
+  MrsUavStateEstimators_Common
+  ${catkin_LIBRARIES}
+  ${Eigen_LIBRARIES}
+  )
+
+## Add configure headers for dynamic reconfigure
+add_dependencies(MrsUavStateEstimators_RtkGarmin
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  ${PROJECT_NAME}_gencfg
+  )
+
 add_library(MrsUavStateEstimators_Passthrough
   src/estimators/state/passthrough.cpp
   )

config/private/rtk_garmin/rtk_garmin.yaml

@@ -0,0 +1,26 @@
+mrs_uav_managers:
+  estimation_manager:
+    rtk_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: false
+
+      estimators: # the names of the partial estimators
+        lateral:
+          name: "lat_rtk"
+        altitude:
+          name: "alt_garmin"
+        heading:
+          name: "hdg_hw_api"
+          passthrough: true # 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

config/public/rtk_garmin/alt_garmin.yaml

@@ -0,0 +1,65 @@
+mrs_uav_managers:
+  estimation_manager:
+    rtk_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)
+
+        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: 0.1 # 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

config/public/rtk_garmin/hdg_hw_api.yaml

@@ -0,0 +1,12 @@
+mrs_uav_managers:
+  estimation_manager:
+    # passthrough estimator - no estimation on its own
+    rtk_garmin: # namespace of the state estimator
+      hdg_hw_api: # namespace of the heading estimator
+
+        max_flight_z: 100.0 # [m] maximum allowed flight Z (in the estimator frame)
+
+        topics:
+          orientation: "hw_api/orientation" # without uav namespace
+          angular_velocity: "hw_api/angular_velocity" # without uav namespace
+

config/public/rtk_garmin/lat_rtk.yaml

@@ -0,0 +1,63 @@
+mrs_uav_managers:
+  estimation_manager:
+    rtk_garmin: # namespace of the state estimator
+      lat_rtk: # namespace of the lateral 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: "mitigate" # {"eland", "switch", "mitigate"}
+
+        hdg_source_topic: "rtk/hdg_hw_api/output" # [mrs_uav_state_estimation/EstimatorOutput]
+
+        repredictor: # repredictor for correct fusion of delayed measurements
+          enabled: false
+
+        process_noise: # process noise covariance (Q)
+          pos: 0.1 # position state
+          vel: 1.0 # velocity state
+          acc: 1.0 # acceleration state
+
+        corrections: [
+          "pos_rtk",
+          "vel_hw_api"
+        ]
+
+        pos_rtk:
+          state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration
+          noise: 0.001 # measurement noise covariance (R)
+          noise_unhealthy_coeff: 1000.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R)
+
+          message:
+            type: "mrs_msgs/RtkGps"
+            topic: "hw_api/rtk" # without uav namespace
+            limit: 
+              delay: 0.5 # [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: ["saturate"] # types of processors attached to this measurement
+
+          # median_filter:
+          #   buffer_size: 100 # [samples] number of historic values from which the median is calculated
+          #   max_diff: 0.5 # [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.2
+            max: 0.2
+            limit: 1.0
+
+        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: 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: [] # types of processors attached to this measurement

config/public/rtk_garmin/rtk_garmin.yaml

@@ -0,0 +1,10 @@
+mrs_uav_managers:
+  estimation_manager:
+    rtk_garmin: # namespace of the state estimator
+      override_frame_id:
+        enabled: false # if true, custom frame_id can be provided instead of the default "[estimator_name]_origin"
+        frame_id: ""
+
+
+
+

estimator_plugins.xml

@@ -16,6 +16,12 @@
   </class>
 </library>
 
+<library path="lib/libMrsUavStateEstimators_RtkGarmin">
+  <class name="mrs_uav_state_estimators/RtkGarmin" type="mrs_uav_state_estimators::rtk_garmin::RtkGarmin" base_class_type="mrs_uav_managers::StateEstimator">
+    <description>RtkGarmin state estimator</description>
+  </class>
+</library>
+
 <library path="lib/libMrsUavStateEstimators_Aloam">
   <class name="mrs_uav_state_estimators/Aloam" type="mrs_uav_state_estimators::aloam::Aloam" base_class_type="mrs_uav_managers::StateEstimator">
     <description>Aloam state estimator</description>

src/estimators/lateral/lat_generic.cpp

@@ -241,7 +241,7 @@ void LatGeneric::timerUpdate(const ros::TimerEvent &event) {
           auto measurement_stamped = res.value(); 
           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
-          ROS_INFO("[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+          ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
                             measurement_stamped.value(AXIS_Y));
         } else {
           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getPrintName().c_str());

src/estimators/state/rtk_garmin.cpp

@@ -0,0 +1,25 @@
+#include <mrs_uav_state_estimators/estimators/state/state_generic.h>
+
+namespace mrs_uav_state_estimators
+{
+
+namespace rtk_garmin
+{
+
+const char estimator_name[] = "rtk_garmin";
+const bool is_core_plugin = true;
+
+class RtkGarmin : public StateGeneric {
+public:
+  RtkGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+  }
+
+  ~RtkGarmin(void) {
+  }
+};
+
+}  // namespace rtk_garmin
+}  // namespace mrs_uav_state_estimators
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::rtk_garmin::RtkGarmin, mrs_uav_managers::StateEstimator)