Skip to content

Commit

Permalink
updated against new param loader
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Oct 13, 2023
1 parent 72c51a6 commit b1b9018
Show file tree
Hide file tree
Showing 20 changed files with 187 additions and 225 deletions.
Empty file added config/private/hdg_hw_api/hdg_hw_api.yaml
View file
Empty file.
2 changes: 1 addition & 1 deletion config/public/gps_garmin/alt_garmin.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ mrs_uav_managers:

excessive_tilt:
orientation_topic: "hw_api/orientation"
max_tilt: deg(30)
max_tilt: 30.0 # [deg]

# Parameters of altitude median filters - buffer_size , max_diff
vel_hw_api:
Expand Down
Empty file added config/public/hdg_hw_api/hdg_hw_api.yaml
View file
Empty file.
2 changes: 1 addition & 1 deletion config/public/hector_garmin/alt_garmin.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ mrs_uav_managers:

excessive_tilt:
orientation_topic: "hw_api/orientation"
max_tilt: deg(30)
max_tilt: 30.0 # [deg]

# Parameters of altitude median filters - buffer_size , max_diff
vel_hw_api:
Expand Down
2 changes: 1 addition & 1 deletion config/public/vio/alt_garmin.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ mrs_uav_managers:

excessive_tilt:
orientation_topic: "hw_api/orientation"
max_tilt: deg(30)
max_tilt: 30.0 # [deg]

# Parameters of altitude median filters - buffer_size , max_diff
vel_hw_api:
Expand Down
56 changes: 30 additions & 26 deletions include/mrs_uav_state_estimators/estimators/correction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include <mrs_uav_managers/estimation_manager/types.h>
#include <mrs_uav_managers/estimation_manager/support.h>
#include <mrs_uav_managers/estimation_manager/common_handlers.h>
#include <mrs_uav_managers/estimation_manager/private_handlers.h>

#include <mrs_uav_state_estimators/processors/processor.h>
#include <mrs_uav_state_estimators/processors/proc_median_filter.h>
Expand Down Expand Up @@ -74,8 +75,9 @@ const std::map<std::string, MessageType_t> map_msg_type{{"nav_msgs/Odometry", Me
template <int n_measurements>
class Correction {

using CommonHandlers_t = mrs_uav_managers::estimation_manager::CommonHandlers_t;
using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;
using CommonHandlers_t = mrs_uav_managers::estimation_manager::CommonHandlers_t;
using PrivateHandlers_t = mrs_uav_managers::estimation_manager::PrivateHandlers_t;
using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;

public:
typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
Expand All @@ -89,7 +91,7 @@ class Correction {

public:
Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& frame_id, const EstimatorType_t& est_type,
const std::shared_ptr<CommonHandlers_t>& ch, std::function<double(int, int)> fun_get_state,
const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction);

std::string getName() const;
Expand Down Expand Up @@ -161,11 +163,12 @@ class Correction {
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_proc_;
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_delay_;

const std::string est_name_;
const std::string name_;
const std::string ns_frame_id_;
const EstimatorType_t est_type_;
std::shared_ptr<CommonHandlers_t> ch_;
const std::string est_name_;
const std::string name_;
const std::string ns_frame_id_;
const EstimatorType_t est_type_;
std::shared_ptr<CommonHandlers_t> ch_;
std::shared_ptr<PrivateHandlers_t> ph_;

MessageType_t msg_type_;
std::string msg_topic_;
Expand Down Expand Up @@ -216,59 +219,60 @@ class Correction {
template <int n_measurements>
Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& ns_frame_id,
const EstimatorType_t& est_type, const std::shared_ptr<CommonHandlers_t>& ch,
std::function<double(int, int)> fun_get_state,
const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction)
: est_name_(est_name),
name_(name),
ns_frame_id_(ns_frame_id),
est_type_(est_type),
ch_(ch),
ph_(ph),
fun_get_state_(fun_get_state),
fun_apply_correction_(fun_apply_correction) {

// | --------------------- load parameters -------------------- |
mrs_lib::ParamLoader param_loader(nh, getPrintName());
param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + getNamespacedName() + "/");
/* param_loader.setPrefix(getNamespacedName() + "/"); */

std::string msg_type_string;
param_loader.loadParam("message/type", msg_type_string);

ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + getNamespacedName() + "/");

ph->param_loader->loadParam("message/type", msg_type_string);
if (map_msg_type.find(msg_type_string) == map_msg_type.end()) {
ROS_ERROR("[%s]: wrong message type: %s of correction %s", getPrintName().c_str(), msg_type_string.c_str(), getName().c_str());
ros::shutdown();
}
msg_type_ = map_msg_type.at(msg_type_string);

param_loader.loadParam("message/topic", msg_topic_);
ph->param_loader->loadParam("message/topic", msg_topic_);
msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
param_loader.loadParam("message/limit/delay", msg_delay_limit_);
ph->param_loader->loadParam("message/limit/delay", msg_delay_limit_);
msg_delay_warn_limit_ = msg_delay_limit_ / 2; // maybe specify this as a param?
param_loader.loadParam("message/limit/time_since_last", time_since_last_msg_limit_);
ph->param_loader->loadParam("message/limit/time_since_last", time_since_last_msg_limit_);

int state_id_tmp;
param_loader.loadParam("state_id", state_id_tmp);
ph->param_loader->loadParam("state_id", state_id_tmp);
if (state_id_tmp < n_StateId_t) {
state_id_ = static_cast<StateId_t>(state_id_tmp);
} else {
ROS_ERROR("[%s]: wrong state id: %d of correction %s", getPrintName().c_str(), state_id_tmp, getName().c_str());
ros::shutdown();
}
if (state_id_ == VELOCITY) {
param_loader.loadParam("body_frame", is_in_body_frame_, true);
ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
}

param_loader.loadParam("noise", R_);
param_loader.loadParam("noise_unhealthy_coeff", R_coeff_);
ph->param_loader->loadParam("noise", R_);
ph->param_loader->loadParam("noise_unhealthy_coeff", R_coeff_);
default_R_ = R_;

// | --------------- processors initialization --------------- |
param_loader.loadParam("processors", processor_names_);
ph->param_loader->loadParam("processors", processor_names_);

for (auto proc_name : processor_names_) {
processors_[proc_name] = createProcessorFromName(proc_name, nh);
}

if (!param_loader.loadedSuccessfully()) {
if (!ph->param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
ros::shutdown();
}
Expand Down Expand Up @@ -1585,13 +1589,13 @@ template <int n_measurements>
std::shared_ptr<Processor<n_measurements>> Correction<n_measurements>::createProcessorFromName(const std::string& name, ros::NodeHandle& nh) {

if (name == "median_filter") {
return std::make_shared<ProcMedianFilter<n_measurements>>(nh, getNamespacedName(), name, ch_);
return std::make_shared<ProcMedianFilter<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
} else if (name == "saturate") {
return std::make_shared<ProcSaturate<n_measurements>>(nh, getNamespacedName(), name, ch_, state_id_, fun_get_state_);
return std::make_shared<ProcSaturate<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_, state_id_, fun_get_state_);
} else if (name == "excessive_tilt") {
return std::make_shared<ProcExcessiveTilt<n_measurements>>(nh, getNamespacedName(), name, ch_);
return std::make_shared<ProcExcessiveTilt<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
} else if (name == "tf_to_world") {
return std::make_shared<ProcTfToWorld<n_measurements>>(nh, getNamespacedName(), name, ch_);
return std::make_shared<ProcTfToWorld<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
} else {
ROS_ERROR("[%s]: requested invalid processor %s", getPrintName().c_str(), name.c_str());
ros::shutdown();
Expand Down
29 changes: 16 additions & 13 deletions include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,11 @@ class ProcExcessiveTilt : public Processor<n_measurements> {
typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;

public:
ProcExcessiveTilt(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch);
ProcExcessiveTilt(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
const std::shared_ptr<PrivateHandlers_t>& ph);

std::tuple<bool, bool> process(measurement_t& measurement) override;
void reset();
void reset();

private:
double max_tilt_sq_;
Expand All @@ -39,19 +40,21 @@ class ProcExcessiveTilt : public Processor<n_measurements> {
/*//{ constructor */
template <int n_measurements>
ProcExcessiveTilt<n_measurements>::ProcExcessiveTilt(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
const std::shared_ptr<CommonHandlers_t>& ch)
: Processor<n_measurements>(nh, correction_name, name, ch) {
const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
: Processor<n_measurements>(nh, correction_name, name, ch, ph) {

// | --------------------- load parameters -------------------- |
mrs_lib::ParamLoader param_loader(nh, Processor<n_measurements>::getPrintName());
param_loader.setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");

param_loader.loadParam("orientation_topic", orientation_topic_);
ph->param_loader->loadParam("orientation_topic", orientation_topic_);
double max_tilt;
param_loader.loadParam("max_tilt", max_tilt);
ph->param_loader->loadParam("max_tilt", max_tilt);

max_tilt = M_PI * (max_tilt / 180.0);

max_tilt_sq_ = std::pow(max_tilt, 2);

if (!param_loader.loadedSuccessfully()) {
if (!ph->param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
ros::shutdown();
}
Expand Down Expand Up @@ -82,7 +85,7 @@ std::tuple<bool, bool> ProcExcessiveTilt<n_measurements>::process(measurement_t&
return {false, false};
}

bool ok_flag = true;
bool ok_flag = true;
bool should_fuse = true;

try {
Expand All @@ -94,14 +97,14 @@ std::tuple<bool, bool> ProcExcessiveTilt<n_measurements>::process(measurement_t&

if (is_excessive_tilt) {
ROS_WARN_THROTTLE(1.0, "[%s]: excessive tilt of %.2f deg. Not fusing correction.", Processor<n_measurements>::getPrintName().c_str(), tilt / M_PI * 180);
ok_flag = false;
ok_flag = false;
should_fuse = false;
}
}
catch (...) {
ROS_ERROR_THROTTLE(1.0, "[%s]: failed obtaining tilt value", Processor<n_measurements>::getPrintName().c_str());
ok_flag = false;
should_fuse = false;
ok_flag = false;
should_fuse = false;
}
return {ok_flag, should_fuse};
}
Expand Down
18 changes: 9 additions & 9 deletions include/mrs_uav_state_estimators/processors/proc_median_filter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,11 @@ class ProcMedianFilter : public Processor<n_measurements> {
typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;

public:
ProcMedianFilter(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch);
ProcMedianFilter(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
const std::shared_ptr<PrivateHandlers_t>& ph);

std::tuple<bool, bool> process(measurement_t& measurement) override;
void reset();
void reset();

private:
std::vector<mrs_lib::MedianFilter> vec_mf_;
Expand All @@ -35,17 +36,16 @@ class ProcMedianFilter : public Processor<n_measurements> {
/*//{ constructor */
template <int n_measurements>
ProcMedianFilter<n_measurements>::ProcMedianFilter(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
const std::shared_ptr<CommonHandlers_t>& ch)
: Processor<n_measurements>(nh, correction_name, name, ch) {
const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
: Processor<n_measurements>(nh, correction_name, name, ch, ph) {

// | --------------------- load parameters -------------------- |
mrs_lib::ParamLoader param_loader(nh, Processor<n_measurements>::getPrintName());
param_loader.setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");

param_loader.loadParam("buffer_size", buffer_size_);
param_loader.loadParam("max_diff", max_diff_);
ph->param_loader->loadParam("buffer_size", buffer_size_);
ph->param_loader->loadParam("max_diff", max_diff_);

if (!param_loader.loadedSuccessfully()) {
if (!ph->param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
ros::shutdown();
}
Expand Down
26 changes: 13 additions & 13 deletions include/mrs_uav_state_estimators/processors/proc_saturate.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,10 @@ class ProcSaturate : public Processor<n_measurements> {

public:
ProcSaturate(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
StateId_t state_id, std::function<double(int, int)> fun_get_state);
const std::shared_ptr<PrivateHandlers_t>& ph, StateId_t state_id, std::function<double(int, int)> fun_get_state);

std::tuple<bool, bool> process(measurement_t& measurement) override;
void reset();
void reset();

private:
const StateId_t state_id_;
Expand All @@ -40,21 +40,21 @@ class ProcSaturate : public Processor<n_measurements> {
/*//{ constructor */
template <int n_measurements>
ProcSaturate<n_measurements>::ProcSaturate(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
const std::shared_ptr<CommonHandlers_t>& ch, const StateId_t state_id, std::function<double(int, int)> fun_get_state)
: Processor<n_measurements>(nh, correction_name, name, ch), state_id_(state_id), fun_get_state_(fun_get_state) {
const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph, const StateId_t state_id,
std::function<double(int, int)> fun_get_state)
: Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {

// | --------------------- load parameters -------------------- |
mrs_lib::ParamLoader param_loader(nh, Processor<n_measurements>::getPrintName());
param_loader.setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");

param_loader.loadParam("start_enabled", this->start_enabled_);
ph->param_loader->loadParam("start_enabled", this->start_enabled_);
this->enabled_ = this->start_enabled_;
param_loader.loadParam("keep_enabled", keep_enabled_);
param_loader.loadParam("min", saturate_min_);
param_loader.loadParam("max", saturate_max_);
param_loader.loadParam("limit", innovation_limit_);
ph->param_loader->loadParam("keep_enabled", keep_enabled_);
ph->param_loader->loadParam("min", saturate_min_);
ph->param_loader->loadParam("max", saturate_max_);
ph->param_loader->loadParam("limit", innovation_limit_);

if (!param_loader.loadedSuccessfully()) {
if (!ph->param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
ros::shutdown();
}
Expand All @@ -77,7 +77,7 @@ std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& meas
ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);

if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
return {true, true}; // do not even try to saturate, trigger innovation-based switch to other estimator
return {true, true}; // do not even try to saturate, trigger innovation-based switch to other estimator
}

if (measurement(i) > state + saturate_max_) {
Expand Down
18 changes: 9 additions & 9 deletions include/mrs_uav_state_estimators/processors/proc_tf_to_world.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,11 @@ class ProcTfToWorld : public Processor<n_measurements> {
typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;

public:
ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch);
ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
const std::shared_ptr<PrivateHandlers_t>& ph);

std::tuple<bool, bool> process(measurement_t& measurement) override;
void reset();
void reset();

private:
bool is_initialized_ = false;
Expand All @@ -44,17 +45,16 @@ class ProcTfToWorld : public Processor<n_measurements> {
/*//{ constructor */
template <int n_measurements>
ProcTfToWorld<n_measurements>::ProcTfToWorld(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
const std::shared_ptr<CommonHandlers_t>& ch)
: Processor<n_measurements>(nh, correction_name, name, ch) {
const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
: Processor<n_measurements>(nh, correction_name, name, ch, ph) {

// | --------------------- load parameters -------------------- |
mrs_lib::ParamLoader param_loader(nh, Processor<n_measurements>::getPrintName());
param_loader.setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");

ph->param_loader->loadParam("gnss_topic", gnss_topic_);

param_loader.loadParam("gnss_topic", gnss_topic_);
gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;

if (!param_loader.loadedSuccessfully()) {
if (!ph->param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
ros::shutdown();
}
Expand Down
Loading

0 comments on commit b1b9018

Please sign in to comment.