Skip to content

Commit

Permalink
added gyro rate correction to aloam hdg estimator
Browse files Browse the repository at this point in the history
  • Loading branch information
@petrlmat
petrlmat committed Oct 18, 2023
1 parent 7ae7097 commit 54dd5db
Show file tree
Hide file tree
Showing 3 changed files with 56 additions and 26 deletions.
2 changes: 1 addition & 1 deletion config/public/aloam/alt_aloam.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ mrs_uav_managers:

pos_aloam:
state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration
noise: 0.001 # measurement noise covariance (R)
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: "nav_msgs/Odometry"
Expand Down
19 changes: 17 additions & 2 deletions config/public/aloam/hdg_aloam.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,13 @@ mrs_uav_managers:
vel: 0.1 # velocity state

corrections: [
"hdg_aloam"
"hdg_aloam",
"hdg_rate_gyro"
]

hdg_aloam:
state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration
noise: 0.0001 # measurement noise covariance (R)
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: "nav_msgs/Odometry"
Expand All @@ -31,3 +32,17 @@ mrs_uav_managers:
time_since_last: 1.0 # [s] larger time step between messages will flag correction as unhealthy

processors: [] # types of processors attached to this measurement

hdg_rate_gyro:
state_id: 1 # 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: "geometry_msgs/Vector3Stamped"
topic: "hw_api/angular_velocity" # without uav namespace
orientation_topic: "hw_api/orientation" # without uav namespace
limit:
delay: 2.0 # [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: [] # types of processors attached to this measurement
61 changes: 38 additions & 23 deletions include/mrs_uav_state_estimators/estimators/correction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,9 @@ class Correction {
std::optional<measurement_t> getCorrectionFromVector(const geometry_msgs::Vector3StampedConstPtr msg);
void callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg);

mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_; // for obtaining heading rate
std::string orientation_topic_;

mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_quat_;
measurement_t prev_hdg_measurement_;
bool got_first_hdg_measurement_ = false;
Expand Down Expand Up @@ -257,7 +260,8 @@ Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& e
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) {

if (state_id_ == StateId_t::VELOCITY) {
ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
}

Expand All @@ -272,11 +276,6 @@ Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& e
processors_[proc_name] = createProcessorFromName(proc_name, nh);
}

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

// | ------------- initialize dynamic reconfigure ------------- |
drmgr_ = std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), getPrintName());
drmgr_->config.noise = R_;
Expand Down Expand Up @@ -339,6 +338,14 @@ Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& e
}
}

// | ------ subscribe orientation for obtaingin hdg rate ------ |
if (est_type_ == EstimatorType_t::HEADING && state_id_ == StateId_t::VELOCITY) {
ph->param_loader->loadParam("message/orientation_topic", orientation_topic_);
orientation_topic_ = "/" + ch_->uav_name + "/" + orientation_topic_;
sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, orientation_topic_);
}


// | --------------- initialize publish handlers -------------- |
if (ch_->debug_topics.correction) {
ph_correction_raw_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/raw", 10);
Expand All @@ -348,6 +355,12 @@ Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& e
ph_delay_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, est_name_ + "/correction/" + getName() + "/delay", 10);
}

// | --- check whether all parameters were loaded correctly --- |
if (!ph->param_loader->loadedSuccessfully()) {
ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
ros::shutdown();
}

healthy_time_ = ros::Time(0);

is_initialized_ = true;
Expand Down Expand Up @@ -1204,7 +1217,7 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
}

default: {
ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
return {};
}
}
Expand All @@ -1229,7 +1242,7 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
}

default: {
ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
return {};
}
}
Expand All @@ -1241,23 +1254,25 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m

switch (state_id_) {

// TODO: needs orientation

/* case StateId_t::VELOCITY: { */
/* try { */
/* measurement_t measurement; */
/* measurement(0) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeadingRate(msg->twist.twist.angular); */
/* return measurement; */
/* } */
/* catch (...) { */
/* ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromOdometry())", getPrintName().c_str()); */
/* return {}; */
/* } */
/* break; */
/* } */
case StateId_t::VELOCITY: {
try {
if (!sh_orientation_.hasMsg()) {
ROS_INFO_THROTTLE(1.0, "[%s]: %s orientation on topic: %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), orientation_topic_.c_str());
return {};
}
measurement_t measurement;
measurement(0) = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
return measurement;
}
catch (...) {
ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromVector())", getPrintName().c_str());
return {};
}
break;
}

default: {
ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
return {};
}
}
Expand Down

0 comments on commit 54dd5db

Please sign in to comment.