From 9fe79080810b8255f279e5ffcdc7b55d618b4e5b Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Wed, 25 Oct 2023 13:36:20 +0200 Subject: [PATCH] removed aloam estimator (will be available as plugin in mrs_aloam_core metapackage) --- config/{public => private}/estimators.yaml | 2 -- config/public/active_estimators.yaml | 3 +-- .../constraint_manager.yaml | 3 --- config/public/gain_manager/gain_manager.yaml | 3 --- .../transform_manager/transform_manager.yaml | 22 ------------------- include/transform_manager/tf_mapping_origin.h | 2 +- launch/estimation_manager.launch | 2 +- src/transform_manager/transform_manager.cpp | 2 +- 8 files changed, 4 insertions(+), 35 deletions(-) rename config/{public => private}/estimators.yaml (92%) diff --git a/config/public/estimators.yaml b/config/private/estimators.yaml similarity index 92% rename from config/public/estimators.yaml rename to config/private/estimators.yaml index 4cb6e891..85baee95 100644 --- a/config/public/estimators.yaml +++ b/config/private/estimators.yaml @@ -10,8 +10,6 @@ mrs_uav_managers: address: "mrs_uav_state_estimators/Rtk" rtk_garmin: address: "mrs_uav_state_estimators/RtkGarmin" - aloam: - address: "mrs_uav_state_estimators/Aloam" vio: address: "mrs_uav_state_estimators/Vio" hector_garmin: diff --git a/config/public/active_estimators.yaml b/config/public/active_estimators.yaml index a80cd7c4..1eb60295 100644 --- a/config/public/active_estimators.yaml +++ b/config/public/active_estimators.yaml @@ -3,12 +3,11 @@ mrs_uav_managers: estimation_manager: # loaded state estimator plugins - # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, aloam, ground_truth, dummy + # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, ground_truth, dummy state_estimators: [ "gps_garmin", "gps_baro", # "rtk", - # "aloam", # "ground_truth", # "dummy" ] diff --git a/config/public/constraint_manager/constraint_manager.yaml b/config/public/constraint_manager/constraint_manager.yaml index 2245b26c..8e010fb9 100644 --- a/config/public/constraint_manager/constraint_manager.yaml +++ b/config/public/constraint_manager/constraint_manager.yaml @@ -7,7 +7,6 @@ mrs_uav_managers: "gps_baro", "rtk", "rtk_garmin", - "aloam", "hector_garmin", "vio", "optflow", @@ -28,7 +27,6 @@ mrs_uav_managers: gps_baro: ["slow", "medium", "fast"] rtk: ["slow", "medium", "fast"] rtk_garmin: ["slow", "medium", "fast"] - aloam: ["slow"] hector_garmin: ["slow"] vio: ["slow"] optflow: ["slow"] @@ -43,7 +41,6 @@ mrs_uav_managers: gps_baro: "slow" rtk: "slow" rtk_garmin: "slow" - aloam: "slow" hector_garmin: "slow" vio: "slow" optflow: "slow" diff --git a/config/public/gain_manager/gain_manager.yaml b/config/public/gain_manager/gain_manager.yaml index 3f313764..9642f99b 100644 --- a/config/public/gain_manager/gain_manager.yaml +++ b/config/public/gain_manager/gain_manager.yaml @@ -7,7 +7,6 @@ mrs_uav_managers: "gps_baro", "rtk", "rtk_garmin", - "aloam", "hector_garmin", "vio", "optflow", @@ -27,7 +26,6 @@ mrs_uav_managers: gps_baro: ["supersoft", "soft"] rtk: ["supersoft", "soft"] rtk_garmin: ["supersoft", "soft"] - aloam: ["supersoft", "soft"] hector_garmin: ["supersoft", "soft"] vio: ["supersoft", "soft"] optflow: ["supersoft", "soft"] @@ -42,7 +40,6 @@ mrs_uav_managers: gps_baro: "soft" rtk: "soft" rtk_garmin: "soft" - aloam: "supersoft" hector_garmin: "supersoft" vio: "supersoft" optflow: "supersoft" diff --git a/config/public/transform_manager/transform_manager.yaml b/config/public/transform_manager/transform_manager.yaml index eeafe7c5..496a1e83 100644 --- a/config/public/transform_manager/transform_manager.yaml +++ b/config/public/transform_manager/transform_manager.yaml @@ -32,18 +32,6 @@ mrs_uav_managers: parent: "fcu" # fcu should be the parent if using the default inverted mrs tf tree convention with fcu as the root of the tf tree child: "fcu_untilted" - mapping_origin_tf: - enabled: true - debug_prints: false - lateral_topic: "slam/odom" # name of the topic used for x, y components of the tf (expects nav_msgs/Odometry topic type) - altitude_topic: "slam/odom" # name of the topic used for z components of the tf (expects nav_msgs/Odometry topic type) - orientation_topic: "hw_api/orientation" # name of the topic used for orientation components of the tf (expects geometry_msgs/Quaternion topic type) - inverted: true # publish as inverted tf (the default for inverted mrs tf tree convention) - custom_frame_id: - enabled: true - frame_id: "mapping_origin" - - # the list of additional source topics from which the tfs will be constructed tf_sources: [] @@ -101,16 +89,6 @@ mrs_uav_managers: inverted: true # publish as inverted tf (the default for inverted mrs tf tree convention) republish_in_frames: [] # the odometry message will be transformed and republished in the specified frames - aloam: - odom_topic: "odom" # name of the topic (expects nav_msgs/Odometry topic type) - tf_from_attitude: # used for transforming velocities before full transform is available - enabled: true - attitude_topic: "attitude" # name of the attitude topic(expects geometry_msgs/QuaternionStamped topic type) - namespace: "estimation_manager/aloam" # the namespace of the topic (usually the node that publishes the topic) - utm_based: false # if true, will publish tf to utm_origin - inverted: true # publish as inverted tf (the default for inverted mrs tf tree convention) - republish_in_frames: [] # the odometry message will be transformed and republished in the specified frames - hector_garmin: odom_topic: "odom" # name of the topic (expects nav_msgs/Odometry topic type) tf_from_attitude: # used for transforming velocities before full transform is available diff --git a/include/transform_manager/tf_mapping_origin.h b/include/transform_manager/tf_mapping_origin.h index 66ad6446..fe1013db 100644 --- a/include/transform_manager/tf_mapping_origin.h +++ b/include/transform_manager/tf_mapping_origin.h @@ -148,7 +148,7 @@ class TfMappingOrigin { const double hdg_mapping_old = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading(); - /* publish aloam mapping origin tf //{ */ + /* publish mapping origin tf //{ */ bool clear_needed = false; diff --git a/launch/estimation_manager.launch b/launch/estimation_manager.launch index d804446d..5bc507c8 100644 --- a/launch/estimation_manager.launch +++ b/launch/estimation_manager.launch @@ -39,7 +39,7 @@ - + diff --git a/src/transform_manager/transform_manager.cpp b/src/transform_manager/transform_manager.cpp index 69825aa1..68cf3c4c 100644 --- a/src/transform_manager/transform_manager.cpp +++ b/src/transform_manager/transform_manager.cpp @@ -330,7 +330,7 @@ void TransformManager::onInit() { // initialize mapping_origin tf bool mapping_origin_tf_enabled; - param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled); + param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled, false); if (mapping_origin_tf_enabled) {