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)