Skip to content

Commit

Permalink
core plugins load configs from this package, non-core load only custo…
Browse files Browse the repository at this point in the history 
…m_config
  • Loading branch information
@petrlmat
petrlmat committed Sep 27, 2023
1 parent 39d79c8 commit fe954bd
Show file tree
Hide file tree
Showing 20 changed files with 121 additions and 83 deletions.
11 changes: 7 additions & 4 deletions include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<AltGeneric> 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);
Expand All @@ -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) {
Expand All @@ -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<double> getHeightCovariance() const override;

mrs_msgs::Float64Stamped getUavAglHeight() const override;
std::vector<double> getHeightCovariance() const override;
};

} // namespace garmin_agl
Expand Down
6 changes: 4 additions & 2 deletions include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ class AltGeneric : public AltitudeEstimator<alt_generic::n_states> {
bool is_repredictor_enabled_;
int rep_buffer_size_ = 200;

const bool is_core_plugin_;

std::vector<std::string> correction_names_;
std::vector<std::shared_ptr<Correction<alt_generic::n_measurements>>> corrections_;

Expand All @@ -104,8 +106,8 @@ class AltGeneric : public AltitudeEstimator<alt_generic::n_states> {
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<alt_generic::n_states>(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<alt_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
}

~AltGeneric(void) {
Expand Down
6 changes: 4 additions & 2 deletions include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,8 @@ class HdgGeneric : public HeadingEstimator<hdg_generic::n_states> {
bool is_repredictor_enabled_;
int rep_buffer_size_ = 200;

const bool is_core_plugin_;

std::vector<std::string> correction_names_;
std::vector<std::shared_ptr<Correction<hdg_generic::n_measurements>>> corrections_;

Expand All @@ -106,8 +108,8 @@ class HdgGeneric : public HeadingEstimator<hdg_generic::n_states> {
double last_valid_hdg_;

public:
HdgGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name)
: HeadingEstimator<hdg_generic::n_states>(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<hdg_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
}

~HdgGeneric(void) {
Expand Down
6 changes: 4 additions & 2 deletions include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,8 @@ class HdgPassthrough : public HeadingEstimator<hdg_passthrough::n_states> {
states_t innovation_;
mutable std::mutex mtx_innovation_;

const bool is_core_plugin_;

std::string orient_topic_;
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_;
void callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg);
Expand All @@ -64,8 +66,8 @@ class HdgPassthrough : public HeadingEstimator<hdg_passthrough::n_states> {
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<hdg_passthrough::n_states>(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<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
}

~HdgPassthrough(void) {
Expand Down
6 changes: 4 additions & 2 deletions include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,8 @@ class LatGeneric : public LateralEstimator<lat_generic::n_states> {
bool is_repredictor_enabled_;
int rep_buffer_size_ = 200;

const bool is_core_plugin_;

std::vector<std::string> correction_names_;
std::vector<std::shared_ptr<Correction<lat_generic::n_measurements>>> corrections_;

Expand Down Expand Up @@ -108,9 +110,9 @@ class LatGeneric : public LateralEstimator<lat_generic::n_states> {
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<std::optional<double>()> fun_get_hdg)
: LateralEstimator<lat_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), fun_get_hdg_(fun_get_hdg) {
: LateralEstimator<lat_generic::n_states>(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) {
Expand Down
11 changes: 7 additions & 4 deletions include/mrs_uav_state_estimators/estimators/state/passthrough.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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<nav_msgs::Odometry> sh_passthrough_odom_;
double _critical_timeout_passthrough_odom_;
std::string msg_topic_;
Expand All @@ -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) {
Expand Down
5 changes: 4 additions & 1 deletion include/mrs_uav_state_estimators/estimators/state/state_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<geometry_msgs::QuaternionStamped> sh_hw_api_orient_;

Expand All @@ -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) {
Expand Down
16 changes: 9 additions & 7 deletions src/estimators/agl/garmin_agl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,16 @@ void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHand
// | --------------------- load parameters -------------------- |
/* mrs_lib::ParamLoader param_loader(nh, getName()); */

bool success = true;
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");
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() + "/"); */
Expand Down Expand Up @@ -66,7 +68,7 @@ void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHand

// | ---------------- estimators initialization --------------- |

est_agl_garmin_ = std::make_unique<AltGeneric>(est_agl_name_, frame_id_, getName());
est_agl_garmin_ = std::make_unique<AltGeneric>(est_agl_name_, frame_id_, getName(), is_core_plugin_);
est_agl_garmin_->initialize(nh, ch_, ph_);

max_flight_z_ = est_agl_garmin_->getMaxFlightZ();
Expand Down
15 changes: 9 additions & 6 deletions src/estimators/altitude/alt_generic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,17 @@ void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan

// | --------------- initialize parameter loader -------------- |
/* Support::loadParamFile(ros::package::getPath(package_name_) + "/config/estimators/" + getNamespacedName() + ".yaml", nh.getNamespace()); */
bool success = true;

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 (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());
Expand Down
28 changes: 16 additions & 12 deletions src/estimators/heading/hdg_generic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,16 @@ void HdgGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan
// clang-format on

// | --------------- initialize parameter loader -------------- |
bool success = true;
if (is_core_plugin_) {
bool success = true;

success *= ph_->loadConfigFile(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());
Expand Down Expand Up @@ -108,7 +110,7 @@ void HdgGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan
}

// | ------------------ timers initialization ----------------- |
timer_update_ = nh.createTimer(ros::Rate(ch_->desired_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 --------------- |
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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
Expand Down
14 changes: 8 additions & 6 deletions src/estimators/heading/hdg_passthrough.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,16 @@ void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<Commo
hdg_covariance_ = covariance_t::Zero();

// | --------------- param loader initialization --------------- |
bool success = true;
if (is_core_plugin_) {
bool success = true;

success *= ph_->loadConfigFile(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());
Expand Down
18 changes: 10 additions & 8 deletions src/estimators/lateral/lat_generic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,16 @@ void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan

// | --------------- initialize parameter loader -------------- |
/* Support::loadParamFile(ros::package::getPath(package_name_) + "/config/estimators/" + getNamespacedName() + ".yaml", nh.getNamespace()); */
bool success = true;
if (is_core_plugin_) {
bool success = true;

success *= ph_->loadConfigFile(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());
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion src/estimators/state/aloam.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
3 changes: 2 additions & 1 deletion src/estimators/state/gps_baro.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
Loading

0 comments on commit fe954bd

Please sign in to comment.