diff --git a/CMakeLists.txt b/CMakeLists.txt
index a499e8e..bb7dc43 100644
--- a/CMakeLists.txt
+++ b/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
)
diff --git a/config/private/rtk_garmin/alt_garmin.yaml b/config/private/rtk_garmin/alt_garmin.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/config/private/rtk_garmin/hdg_hw_api.yaml b/config/private/rtk_garmin/hdg_hw_api.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/config/private/rtk_garmin/lat_rtk.yaml b/config/private/rtk_garmin/lat_rtk.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/config/private/rtk_garmin/rtk_garmin.yaml b/config/private/rtk_garmin/rtk_garmin.yaml
new file mode 100644
index 0000000..f7bb5fb
--- /dev/null
+++ b/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
diff --git a/config/public/rtk_garmin/alt_garmin.yaml b/config/public/rtk_garmin/alt_garmin.yaml
new file mode 100644
index 0000000..a6ee3b1
--- /dev/null
+++ b/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
diff --git a/config/public/rtk_garmin/hdg_hw_api.yaml b/config/public/rtk_garmin/hdg_hw_api.yaml
new file mode 100644
index 0000000..da94a24
--- /dev/null
+++ b/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
+
diff --git a/config/public/rtk_garmin/lat_rtk.yaml b/config/public/rtk_garmin/lat_rtk.yaml
new file mode 100644
index 0000000..75dbdc3
--- /dev/null
+++ b/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
diff --git a/config/public/rtk_garmin/rtk_garmin.yaml b/config/public/rtk_garmin/rtk_garmin.yaml
new file mode 100644
index 0000000..5f71b2a
--- /dev/null
+++ b/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: ""
+
+
+
+
diff --git a/estimator_plugins.xml b/estimator_plugins.xml
index 3f384e3..d0221f7 100644
--- a/estimator_plugins.xml
+++ b/estimator_plugins.xml
@@ -16,6 +16,12 @@
+
+
+ RtkGarmin state estimator
+
+
+
Aloam state estimator
diff --git a/src/estimators/lateral/lat_generic.cpp b/src/estimators/lateral/lat_generic.cpp
index 30059da..f00bd13 100644
--- a/src/estimators/lateral/lat_generic.cpp
+++ b/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());
diff --git a/src/estimators/state/rtk_garmin.cpp b/src/estimators/state/rtk_garmin.cpp
new file mode 100644
index 0000000..b867a53
--- /dev/null
+++ b/src/estimators/state/rtk_garmin.cpp
@@ -0,0 +1,25 @@
+#include
+
+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_EXPORT_CLASS(mrs_uav_state_estimators::rtk_garmin::RtkGarmin, mrs_uav_managers::StateEstimator)