Skip to content

Commit

Permalink
update state as soon as passthrough odometry arrives
Browse files Browse the repository at this point in the history
  • Loading branch information
@petrlmat
petrlmat committed Aug 28, 2024
1 parent 4042562 commit 5a592d7
Show file tree
Hide file tree
Showing 2 changed files with 144 additions and 30 deletions.
12 changes: 9 additions & 3 deletions include/mrs_uav_state_estimators/estimators/state/passthrough.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ class Passthrough : public mrs_uav_managers::StateEstimator {
using CommonHandlers_t = mrs_uav_managers::estimation_manager::CommonHandlers_t;

private:

const std::string package_name_ = "mrs_uav_state_estimators";

const std::string est_lat_name_ = "lat_passthrough";
Expand All @@ -48,15 +49,20 @@ class Passthrough : public mrs_uav_managers::StateEstimator {
const bool is_core_plugin_;

mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_passthrough_odom_;
void callbackPassthroughOdom(const nav_msgs::Odometry::ConstPtr msg);
double _critical_timeout_passthrough_odom_;
std::string msg_topic_;

ros::Timer timer_check_passthrough_odom_hz_;
void timerCheckPassthroughOdomHz(const ros::TimerEvent &event);
std::atomic<int> counter_odom_msgs_ = 0;
ros::WallTime t_check_hz_last_;
double prev_avg_hz_ = 0;

ros::Timer timer_update_;
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);

bool isConverged();

Expand All @@ -69,7 +75,7 @@ class Passthrough : public mrs_uav_managers::StateEstimator {
~Passthrough(void) {
}

void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
void initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
bool start(void) override;
bool pause(void) override;
bool reset(void) override;
Expand Down
162 changes: 135 additions & 27 deletions src/estimators/state/passthrough.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,32 +11,30 @@ namespace passthrough
{

/* initialize() //{*/
void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
void Passthrough::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {

ch_ = ch;
ph_ = ph;

ros::NodeHandle nh(parent_nh);

ns_frame_id_ = ch_->uav_name + "/" + frame_id_;

// | --------------- param loader initialization -------------- |

if (is_core_plugin_) {

ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
}

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

// | --------------------- load parameters -------------------- |
ph->param_loader->loadParam("max_flight_z", max_flight_z_);
ph->param_loader->loadParam("message/topic", msg_topic_);
ph_->param_loader->loadParam("max_flight_z", max_flight_z_);
ph_->param_loader->loadParam("message/topic", msg_topic_);
msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;

// | ------------------ timers initialization ----------------- |
timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerUpdate, this, false, false); // not running after init
timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerCheckHealth, this);

// | --------------- subscribers initialization --------------- |
mrs_lib::SubscribeHandlerOptions shopts;
shopts.nh = nh;
Expand All @@ -47,7 +45,12 @@ void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHa
shopts.queue_size = 10;
shopts.transport_hints = ros::TransportHints().tcpNoDelay();

sh_passthrough_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_);
sh_passthrough_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_, &Passthrough::callbackPassthroughOdom, this);

// | ------------------ timers initialization ----------------- |
t_check_hz_last_ = ros::WallTime::now();
prev_avg_hz_ = 0;
timer_check_passthrough_odom_hz_ = nh.createTimer(ros::Rate(1.0), &Passthrough::timerCheckPassthroughOdomHz, this);

// | ---------------- publishers initialization --------------- |
ph_odom_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10); // needed for tf
Expand Down Expand Up @@ -93,7 +96,7 @@ bool Passthrough::start(void) {

if (isInState(READY_STATE)) {

timer_update_.start();
/* timer_update_.start(); */
changeState(STARTED_STATE);
return true;

Expand Down Expand Up @@ -135,12 +138,65 @@ bool Passthrough::reset(void) {
}
/*//}*/

/* timerCheckPassthroughOdomHz() //{*/
void Passthrough::timerCheckPassthroughOdomHz([[maybe_unused]] const ros::TimerEvent &event) {

if (!isInitialized()) {
return;
}

if (isInState(INITIALIZED_STATE)) {

// first wait for a message
if (!sh_passthrough_odom_.hasMsg()) {
ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_passthrough_odom_.topicName().c_str());
return;
}

// calculate average rate of messages
if (counter_odom_msgs_ > 0) {
ros::WallTime t_now = ros::WallTime::now();
double dt = (t_now - t_check_hz_last_).toSec();
double avg_hz = counter_odom_msgs_ / dt;
t_check_hz_last_ = t_now;
counter_odom_msgs_ = 0;
ROS_INFO("[%s]: rate of passthrough odom: %.2f Hz", getPrintName().c_str(), avg_hz);

// check message rate stability
if (abs(avg_hz - prev_avg_hz_) >= 5) {
ROS_INFO("[%s]: %s stable passthrough odometry rate. now: %.2f Hz prev: %.2f Hz", getPrintName().c_str(), Support::waiting_for_string.c_str(), avg_hz,
prev_avg_hz_);
prev_avg_hz_ = avg_hz;
return;
}

// the message rate must be higher than required by the control manager
if (avg_hz < ch_->desired_uav_state_rate) {
ROS_ERROR(
"[%s]: rate of passthrough odom: %.2f Hz lower than desired uav_state rate: %.2f Hz. Flight not allowed. Provide higher passthrough odometry rate "
"or use a higher-level controller.",
getPrintName().c_str(), avg_hz, ch_->desired_uav_state_rate);
// note: might run the publishing asynchronously on the desired rate in this case to still be able to fly
// the states would then be interpolated by ZOH (bleeh)
/* timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerUpdate, this); */
return;
}
}

changeState(READY_STATE);
ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
timer_check_passthrough_odom_hz_.stop();
}
}
/*//}*/

/* timerUpdate() //{*/
void Passthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {


if (!isInitialized()) {
return;
if (isInState(STARTED_STATE)) {

changeState(RUNNING_STATE);
}

const ros::Time time_now = ros::Time::now();
Expand Down Expand Up @@ -202,29 +258,81 @@ void Passthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
}
/*//}*/

/*//{ timerCheckHealth() */
void Passthrough::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
/*//{ callbackPassthroughOdom() */
void Passthrough::callbackPassthroughOdom(const nav_msgs::Odometry::ConstPtr msg) {

counter_odom_msgs_++;

if (!isInitialized()) {
return;
}

if (isInState(INITIALIZED_STATE)) {
if (isInState(STARTED_STATE)) {

if (sh_passthrough_odom_.hasMsg()) {
changeState(READY_STATE);
ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
} else {
ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_passthrough_odom_.topicName().c_str());
return;
}
changeState(RUNNING_STATE);
}

if (first_iter_) {
prev_msg_ = msg;
first_iter_ = false;
}

if (isInState(STARTED_STATE)) {
nav_msgs::Odometry odom = *msg;

changeState(RUNNING_STATE);
}
// change the frame_ids
odom.header.frame_id = ns_frame_id_;
odom.child_frame_id = ch_->frames.ns_fcu;

mrs_msgs::UavState uav_state = uav_state_init_;

// here we are assuming the passthrough odometry has correct timestamp
uav_state.header.stamp = odom.header.stamp;
/* const ros::Time time_now = ros::Time::now(); */
/* uav_state.header.stamp = time_now; */

uav_state.pose.position = odom.pose.pose.position;
uav_state.pose.orientation = odom.pose.pose.orientation;
uav_state.velocity.angular = odom.twist.twist.angular;

// uav_state has velocities in the parent frame
uav_state.velocity.linear = Support::rotateVector(odom.twist.twist.linear, odom.pose.pose.orientation);


nav_msgs::Odometry innovation = innovation_init_;
innovation.header.stamp = odom.header.stamp;

innovation.pose.pose.position.x = prev_msg_->pose.pose.position.x - msg->pose.pose.position.x;
innovation.pose.pose.position.y = prev_msg_->pose.pose.position.y - msg->pose.pose.position.y;
innovation.pose.pose.position.z = prev_msg_->pose.pose.position.z - msg->pose.pose.position.z;

mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
pose_covariance.header.stamp = odom.header.stamp;
twist_covariance.header.stamp = odom.header.stamp;

const int n_states = 6; // TODO this should be defined somewhere else
pose_covariance.values.resize(n_states * n_states);
pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;

twist_covariance.values.resize(n_states * n_states);
twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;

mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);

publishUavState();
publishOdom();
publishCovariance();
publishInnovation();
publishDiagnostics();

prev_msg_ = msg;
}
/*//}*/

Expand Down

0 comments on commit 5a592d7

Please sign in to comment.