diff --git a/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h b/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h index bc9f779..9bdcdb3 100644 --- a/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h +++ b/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h @@ -30,12 +30,16 @@ const char name[] = "garmin_agl"; const char frame_id[] = "garmin_agl_origin"; const char package_name[] = "mrs_uav_state_estimators"; +const bool is_core_plugin = true; + class GarminAgl : public mrs_uav_managers::AglEstimator { private: std::unique_ptr est_agl_garmin_; const std::string est_agl_name_ = "garmin_agl"; + const bool is_core_plugin_; + ros::Timer timer_update_; int _update_timer_rate_; void timerUpdate(const ros::TimerEvent &event); @@ -49,7 +53,7 @@ class GarminAgl : public mrs_uav_managers::AglEstimator { void waitForEstimationInitialization(); public: - GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name) { + GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name), is_core_plugin_(is_core_plugin) { } ~GarminAgl(void) { @@ -60,9 +64,8 @@ class GarminAgl : public mrs_uav_managers::AglEstimator { bool pause(void) override; bool reset(void) override; - mrs_msgs::Float64Stamped getUavAglHeight() const override; - std::vector getHeightCovariance() const override; - + mrs_msgs::Float64Stamped getUavAglHeight() const override; + std::vector getHeightCovariance() const override; }; } // namespace garmin_agl diff --git a/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h b/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h index 577e07e..aeb7c1b 100644 --- a/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h +++ b/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h @@ -82,6 +82,8 @@ class AltGeneric : public AltitudeEstimator { bool is_repredictor_enabled_; int rep_buffer_size_ = 200; + const bool is_core_plugin_; + std::vector correction_names_; std::vector>> corrections_; @@ -104,8 +106,8 @@ class AltGeneric : public AltitudeEstimator { mutable std::mutex mtx_Q_; public: - AltGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name) - : AltitudeEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name) { + AltGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin) + : AltitudeEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) { } ~AltGeneric(void) { diff --git a/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h b/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h index 3c3dd1f..7b94bec 100644 --- a/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h +++ b/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h @@ -81,6 +81,8 @@ class HdgGeneric : public HeadingEstimator { bool is_repredictor_enabled_; int rep_buffer_size_ = 200; + const bool is_core_plugin_; + std::vector correction_names_; std::vector>> corrections_; @@ -106,8 +108,8 @@ class HdgGeneric : public HeadingEstimator { double last_valid_hdg_; public: - HdgGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name) - : HeadingEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name) { + HdgGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin) + : HeadingEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) { } ~HdgGeneric(void) { diff --git a/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h b/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h index 7b91898..cb91fd5 100644 --- a/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h +++ b/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h @@ -47,6 +47,8 @@ class HdgPassthrough : public HeadingEstimator { states_t innovation_; mutable std::mutex mtx_innovation_; + const bool is_core_plugin_; + std::string orient_topic_; mrs_lib::SubscribeHandler sh_orientation_; void callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg); @@ -64,8 +66,8 @@ class HdgPassthrough : public HeadingEstimator { void timerCheckHealth(const ros::TimerEvent &event); public: - HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name) - : HeadingEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name) { + HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin) + : HeadingEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) { } ~HdgPassthrough(void) { diff --git a/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h b/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h index e688dd7..323929d 100644 --- a/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h +++ b/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h @@ -79,6 +79,8 @@ class LatGeneric : public LateralEstimator { bool is_repredictor_enabled_; int rep_buffer_size_ = 200; + const bool is_core_plugin_; + std::vector correction_names_; std::vector>> corrections_; @@ -108,9 +110,9 @@ class LatGeneric : public LateralEstimator { mutable std::mutex mtx_Q_; public: - LatGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, + LatGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin, std::function()> fun_get_hdg) - : LateralEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), fun_get_hdg_(fun_get_hdg) { + : LateralEstimator(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin), fun_get_hdg_(fun_get_hdg) { } ~LatGeneric(void) { diff --git a/include/mrs_uav_state_estimators/estimators/state/passthrough.h b/include/mrs_uav_state_estimators/estimators/state/passthrough.h index 32ef1e0..ae293da 100644 --- a/include/mrs_uav_state_estimators/estimators/state/passthrough.h +++ b/include/mrs_uav_state_estimators/estimators/state/passthrough.h @@ -28,6 +28,8 @@ const char name[] = "passthrough"; const char frame_id[] = "passthrough_origin"; const char package_name[] = "mrs_uav_state_estimators"; +const bool is_core_plugin = true; + using namespace mrs_uav_managers::estimation_manager; class Passthrough : public mrs_uav_managers::StateEstimator { @@ -43,6 +45,8 @@ class Passthrough : public mrs_uav_managers::StateEstimator { const std::string est_hdg_name_ = "hdg_passthrough"; + const bool is_core_plugin_; + mrs_lib::SubscribeHandler sh_passthrough_odom_; double _critical_timeout_passthrough_odom_; std::string msg_topic_; @@ -51,16 +55,15 @@ class Passthrough : public mrs_uav_managers::StateEstimator { void timerUpdate(const ros::TimerEvent &event); nav_msgs::OdometryConstPtr prev_msg_; bool first_iter_ = true; - - ros::Timer timer_check_health_; - void timerCheckHealth(const ros::TimerEvent &event); + ros::Timer timer_check_health_; + void timerCheckHealth(const ros::TimerEvent &event); bool isConverged(); void waitForEstimationInitialization(); public: - Passthrough() : StateEstimator(passthrough::name, passthrough::frame_id, passthrough::package_name) { + Passthrough() : StateEstimator(passthrough::name, passthrough::frame_id, passthrough::package_name), is_core_plugin_(is_core_plugin) { } ~Passthrough(void) { diff --git a/include/mrs_uav_state_estimators/estimators/state/state_generic.h b/include/mrs_uav_state_estimators/estimators/state/state_generic.h index f12a32a..f7ca807 100644 --- a/include/mrs_uav_state_estimators/estimators/state/state_generic.h +++ b/include/mrs_uav_state_estimators/estimators/state/state_generic.h @@ -53,6 +53,8 @@ class StateGeneric : public mrs_uav_managers::StateEstimator { bool is_override_frame_id_; + const bool is_core_plugin_; + std::string topic_orientation_; mrs_lib::SubscribeHandler sh_hw_api_orient_; @@ -73,7 +75,8 @@ class StateGeneric : public mrs_uav_managers::StateEstimator { void waitForEstimationInitialization(); public: - StateGeneric(const std::string &name) : StateEstimator(name, name + "_origin", state_generic::package_name) { + StateGeneric(const std::string &name, const bool is_core_plugin) + : StateEstimator(name, name + "_origin", state_generic::package_name), is_core_plugin_(is_core_plugin) { } ~StateGeneric(void) { diff --git a/src/estimators/agl/garmin_agl.cpp b/src/estimators/agl/garmin_agl.cpp index af7a241..05e9341 100644 --- a/src/estimators/agl/garmin_agl.cpp +++ b/src/estimators/agl/garmin_agl.cpp @@ -22,14 +22,16 @@ void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptrloadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); - if (!success) { - ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); - ros::shutdown(); + if (!success) { + ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); + ros::shutdown(); + } } /* param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/"); */ @@ -66,7 +68,7 @@ void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr(est_agl_name_, frame_id_, getName()); + est_agl_garmin_ = std::make_unique(est_agl_name_, frame_id_, getName(), is_core_plugin_); est_agl_garmin_->initialize(nh, ch_, ph_); max_flight_z_ = est_agl_garmin_->getMaxFlightZ(); diff --git a/src/estimators/altitude/alt_generic.cpp b/src/estimators/altitude/alt_generic.cpp index 46857b5..4736560 100644 --- a/src/estimators/altitude/alt_generic.cpp +++ b/src/estimators/altitude/alt_generic.cpp @@ -33,14 +33,17 @@ void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptrloadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); + if (is_core_plugin_) { + bool success = true; - if (!success) { - ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); - ros::shutdown(); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); + + if (!success) { + ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); + ros::shutdown(); + } } mrs_lib::ParamLoader param_loader(nh, getPrintName()); diff --git a/src/estimators/heading/hdg_generic.cpp b/src/estimators/heading/hdg_generic.cpp index 8641c70..dfc7403 100644 --- a/src/estimators/heading/hdg_generic.cpp +++ b/src/estimators/heading/hdg_generic.cpp @@ -32,14 +32,16 @@ void HdgGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptrloadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); - if (!success) { - ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); - ros::shutdown(); + if (!success) { + ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); + ros::shutdown(); + } } mrs_lib::ParamLoader param_loader(nh, getPrintName()); @@ -108,7 +110,7 @@ void HdgGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptrdesired_uav_state_rate), &HdgGeneric::timerUpdate, this); // not running after init + timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgGeneric::timerUpdate, this); // not running after init /* timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgGeneric::timerCheckHealth, this); */ // | --------------- subscribers initialization --------------- | @@ -232,7 +234,8 @@ void HdgGeneric::timerUpdate(const ros::TimerEvent &event) { setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X); ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X)); } else { - ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getNamespacedName().c_str()); + ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), + correction->getNamespacedName().c_str()); return; } } @@ -307,7 +310,7 @@ void HdgGeneric::timerUpdate(const ros::TimerEvent &event) { return; } - if (!is_repredictor_enabled_) { // repredictor requires constant dt + if (!is_repredictor_enabled_) { // repredictor requires constant dt setDt(dt); } @@ -391,7 +394,8 @@ void HdgGeneric::timerCheckHealth(const ros::TimerEvent &event) { setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X); ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X)); } else { - ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getNamespacedName().c_str()); + ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), + correction->getNamespacedName().c_str()); return; } } @@ -466,10 +470,10 @@ void HdgGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_i if (!isInitialized()) { return; } - + // we do not want to perform corrections until the estimator is initialized if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) { - return; + return; } // for position state check the innovation diff --git a/src/estimators/heading/hdg_passthrough.cpp b/src/estimators/heading/hdg_passthrough.cpp index 3651fe8..2959e78 100644 --- a/src/estimators/heading/hdg_passthrough.cpp +++ b/src/estimators/heading/hdg_passthrough.cpp @@ -22,14 +22,16 @@ void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptrloadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); - if (!success) { - ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); - ros::shutdown(); + if (!success) { + ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); + ros::shutdown(); + } } mrs_lib::ParamLoader param_loader(nh, getPrintName()); diff --git a/src/estimators/lateral/lat_generic.cpp b/src/estimators/lateral/lat_generic.cpp index 06f7ffd..5f13f77 100644 --- a/src/estimators/lateral/lat_generic.cpp +++ b/src/estimators/lateral/lat_generic.cpp @@ -33,14 +33,16 @@ void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptrloadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getNamespacedName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getNamespacedName() + ".yaml"); - if (!success) { - ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); - ros::shutdown(); + if (!success) { + ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); + ros::shutdown(); + } } mrs_lib::ParamLoader param_loader(nh, getPrintName()); @@ -327,7 +329,7 @@ void LatGeneric::timerUpdate(const ros::TimerEvent &event) { return; } - if (!is_repredictor_enabled_) { // repredictor requires constant dt TODO: how to handle repredictor + variable rate? + if (!is_repredictor_enabled_) { // repredictor requires constant dt TODO: how to handle repredictor + variable rate? setDt(dt); } @@ -505,7 +507,7 @@ void LatGeneric::doCorrection(const z_t &z, const double R, const StateId_t &sta // we do not want to perform corrections until the estimator is initialized if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) { - return; + return; } // for position state check the innovation diff --git a/src/estimators/state/aloam.cpp b/src/estimators/state/aloam.cpp index 677ed81..8f52750 100644 --- a/src/estimators/state/aloam.cpp +++ b/src/estimators/state/aloam.cpp @@ -8,10 +8,11 @@ namespace aloam { const char estimator_name[] = "aloam"; +const bool is_core_plugin = true; class Aloam : public StateGeneric { public: - Aloam() : StateGeneric(estimator_name) { + Aloam() : StateGeneric(estimator_name, is_core_plugin) { } ~Aloam(void) { diff --git a/src/estimators/state/gps_baro.cpp b/src/estimators/state/gps_baro.cpp index 488988d..bf55a64 100644 --- a/src/estimators/state/gps_baro.cpp +++ b/src/estimators/state/gps_baro.cpp @@ -7,10 +7,11 @@ namespace gps_baro { const char estimator_name[] = "gps_baro"; +const bool is_core_plugin = true; class GpsBaro : public StateGeneric { public: - GpsBaro() : StateGeneric(estimator_name) { + GpsBaro() : StateGeneric(estimator_name, is_core_plugin) { } ~GpsBaro(void) { diff --git a/src/estimators/state/gps_garmin.cpp b/src/estimators/state/gps_garmin.cpp index 7d5b187..298c5f2 100644 --- a/src/estimators/state/gps_garmin.cpp +++ b/src/estimators/state/gps_garmin.cpp @@ -7,10 +7,11 @@ namespace gps_garmin { const char estimator_name[] = "gps_garmin"; +const bool is_core_plugin = true; class GpsGarmin : public StateGeneric { public: - GpsGarmin() : StateGeneric(estimator_name) { + GpsGarmin() : StateGeneric(estimator_name, is_core_plugin) { } ~GpsGarmin(void) { diff --git a/src/estimators/state/hector_garmin.cpp b/src/estimators/state/hector_garmin.cpp index 1e2a040..58a5f90 100644 --- a/src/estimators/state/hector_garmin.cpp +++ b/src/estimators/state/hector_garmin.cpp @@ -7,10 +7,11 @@ namespace hector_garmin { const char estimator_name[] = "hector_garmin"; +const bool is_core_plugin = true; class HectorGarmin : public StateGeneric { public: - HectorGarmin() : StateGeneric(estimator_name) { + HectorGarmin() : StateGeneric(estimator_name, is_core_plugin) { } ~HectorGarmin(void) { diff --git a/src/estimators/state/passthrough.cpp b/src/estimators/state/passthrough.cpp index ec043e8..ff92a01 100644 --- a/src/estimators/state/passthrough.cpp +++ b/src/estimators/state/passthrough.cpp @@ -21,14 +21,16 @@ void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptrloadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); - if (!success) { - ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); - ros::shutdown(); + if (!success) { + ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); + ros::shutdown(); + } } param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/"); @@ -55,7 +57,7 @@ void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr(shopts, msg_topic_); // | ---------------- publishers initialization --------------- | - ph_odom_ = mrs_lib::PublisherHandler(nh, Support::toSnakeCase(getName()) + "/odom", 10); // needed for tf + ph_odom_ = mrs_lib::PublisherHandler(nh, Support::toSnakeCase(getName()) + "/odom", 10); // needed for tf if (ch_->debug_topics.state) { ph_uav_state_ = mrs_lib::PublisherHandler(nh, Support::toSnakeCase(getName()) + "/uav_state", 10); } diff --git a/src/estimators/state/rtk.cpp b/src/estimators/state/rtk.cpp index 4ea0013..e91cc20 100644 --- a/src/estimators/state/rtk.cpp +++ b/src/estimators/state/rtk.cpp @@ -7,10 +7,11 @@ namespace rtk { const char estimator_name[] = "rtk"; +const bool is_core_plugin = true; class Rtk : public StateGeneric { public: - Rtk() : StateGeneric(estimator_name) { + Rtk() : StateGeneric(estimator_name, is_core_plugin) { } ~Rtk(void) { diff --git a/src/estimators/state/state_generic.cpp b/src/estimators/state/state_generic.cpp index cc9a5b9..1e791e2 100644 --- a/src/estimators/state/state_generic.cpp +++ b/src/estimators/state/state_generic.cpp @@ -15,14 +15,14 @@ void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr< ros::NodeHandle nh(parent_nh); - bool success = true; - - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); - success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); - - if (!success) { - ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); - ros::shutdown(); + if (is_core_plugin_) { + bool success = true; + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); + success *= ph_->loadConfigFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); + if (!success) { + ROS_ERROR("[%s]: could not load config file", getPrintName().c_str()); + ros::shutdown(); + } } mrs_lib::ParamLoader param_loader(nh, getPrintName()); @@ -54,7 +54,7 @@ void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr< ns_frame_id_ = ch_->uav_name + "/" + frame_id_; // | ------------------ timers initialization ----------------- | - timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerUpdate, this); // not running after init + timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerUpdate, this); // not running after init /* timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerCheckHealth, this); */ timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this); @@ -93,18 +93,18 @@ void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr< std::vector max_altitudes; if (is_hdg_passthrough_) { - est_hdg_ = std::make_unique(est_hdg_name_, frame_id_, getName()); + est_hdg_ = std::make_unique(est_hdg_name_, frame_id_, getName(), is_core_plugin_); } else { - est_hdg_ = std::make_unique(est_hdg_name_, frame_id_, getName()); + est_hdg_ = std::make_unique(est_hdg_name_, frame_id_, getName(), is_core_plugin_); } est_hdg_->initialize(nh, ch_, ph_); max_altitudes.push_back(est_hdg_->getMaxFlightZ()); - est_lat_ = std::make_unique(est_lat_name_, frame_id_, getName(), [this](void) { return this->getHeading(); }); + est_lat_ = std::make_unique(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); }); est_lat_->initialize(nh, ch_, ph_); max_altitudes.push_back(est_lat_->getMaxFlightZ()); - est_alt_ = std::make_unique(est_alt_name_, frame_id_, getName()); + est_alt_ = std::make_unique(est_alt_name_, frame_id_, getName(), is_core_plugin_); est_alt_->initialize(nh, ch_, ph_); max_altitudes.push_back(est_alt_->getMaxFlightZ()); @@ -217,7 +217,7 @@ void StateGeneric::timerUpdate(const ros::TimerEvent &event) { if (!isInitialized()) { return; } - + mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled); switch (getCurrentSmState()) { diff --git a/src/estimators/state/vio.cpp b/src/estimators/state/vio.cpp index 0cc3cc0..c2ea9c5 100644 --- a/src/estimators/state/vio.cpp +++ b/src/estimators/state/vio.cpp @@ -7,10 +7,11 @@ namespace vio { const char estimator_name[] = "vio"; +const bool is_core_plugin = true; class Vio : public StateGeneric { public: - Vio() : StateGeneric(estimator_name) { + Vio() : StateGeneric(estimator_name, is_core_plugin) { } ~Vio(void) {