Skip to content

Commit

Permalink
add on-demand prediction
Browse files Browse the repository at this point in the history
  • Loading branch information
@henrygerardmoore
henrygerardmoore committed Dec 4, 2024
1 parent 31e5eb9 commit 6c6f9aa
Show file tree
Hide file tree
Showing 5 changed files with 181 additions and 124 deletions.
9 changes: 9 additions & 0 deletions fuse_models/include/fuse_models/odometry_3d_publisher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,12 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher
const std::string& name) override;

protected:
// helper function
void predict(tf2::Transform& pose, nav_msgs::msg::Odometry& odom_output, rclcpp::Time const& to_predict_to,
geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output, bool latest_covariance_valid) const;
void publishTF(nav_msgs::msg::Odometry const& odom_output, tf2::Transform& pose);

void predictTimestampCallback(builtin_interfaces::msg::Time const& time_msg);
/**
* @brief Perform any required post-construction initialization, such as advertising publishers or
* reading from the parameter server.
Expand Down Expand Up @@ -221,6 +227,9 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher

rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
rclcpp::Publisher<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr acceleration_pub_;
rclcpp::Subscription<builtin_interfaces::msg::Time>::SharedPtr predict_timestamp_sub_;
rclcpp::Time predict_timestamp_;
std::mutex predict_timestamp_mutex_;

std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ = nullptr;
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
Expand Down
6 changes: 6 additions & 0 deletions fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,10 @@ struct Odometry3DPublisherParams : public ParameterBase
{
publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf);
invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf);
predict_to_future =
fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "predict_to_future"), predict_to_future);
predict_timestamp_topic = fuse_core::getParam(
interfaces, fuse_core::joinParameterName(ns, "predict_timestamp_topic"), predict_timestamp_topic);
predict_to_current_time = fuse_core::getParam(
interfaces, fuse_core::joinParameterName(ns, "predict_to_current_time"), predict_to_current_time);
predict_with_acceleration = fuse_core::getParam(
Expand Down Expand Up @@ -156,6 +160,8 @@ struct Odometry3DPublisherParams : public ParameterBase
std::string base_link_output_frame_id{ base_link_frame_id };
std::string world_frame_id{ odom_frame_id };
std::string topic{ "odometry/filtered" };
bool predict_to_future{ false };
std::string predict_timestamp_topic{ "predict_time" };
std::string acceleration_topic{ "acceleration/filtered" };
ceres::Covariance::Options covariance_options;
};
Expand Down
283 changes: 159 additions & 124 deletions fuse_models/src/odometry_3d_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@
#include <chrono>
#include <memory>
#include <mutex>
#include <rclcpp/create_subscription.hpp>
#include <rclcpp/time.hpp>
#include <string>
#include <utility>
#include <vector>
Expand Down Expand Up @@ -114,6 +116,16 @@ void Odometry3DPublisher::onInit()
rclcpp::create_publisher<nav_msgs::msg::Odometry>(interfaces_, params_.topic, params_.queue_size, pub_options);
acceleration_pub_ = rclcpp::create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>(
interfaces_, params_.acceleration_topic, params_.queue_size, pub_options);

predict_timestamp_sub_ = rclcpp::create_subscription<builtin_interfaces::msg::Time>(
interfaces_, params_.predict_timestamp_topic, params_.queue_size,
std::bind(&Odometry3DPublisher::predictTimestampCallback, this, std::placeholders::_1));
}

void Odometry3DPublisher::predictTimestampCallback(builtin_interfaces::msg::Time const& time_msg)
{
std::lock_guard<std::mutex> const lock(predict_timestamp_mutex_);
predict_timestamp_ = rclcpp::Time(time_msg);
}

void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction,
Expand Down Expand Up @@ -420,23 +432,157 @@ bool Odometry3DPublisher::getState(const fuse_core::Graph& graph, const rclcpp::
return true;
}

void Odometry3DPublisher::predict(tf2::Transform& pose, nav_msgs::msg::Odometry& odom_output,
rclcpp::Time const& to_predict_to,
geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output,
bool latest_covariance_valid) const
{
const double dt = to_predict_to.seconds() - rclcpp::Time(odom_output.header.stamp).seconds();
// Convert pose in Eigen representation
fuse_core::Vector3d position;
fuse_core::Vector3d velocity_linear;
fuse_core::Vector3d velocity_angular;
Eigen::Quaterniond orientation;
position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z();
orientation.x() = pose.getRotation().x();
orientation.y() = pose.getRotation().y();
orientation.z() = pose.getRotation().z();
orientation.w() = pose.getRotation().w();
velocity_linear << odom_output.twist.twist.linear.x, odom_output.twist.twist.linear.y,
odom_output.twist.twist.linear.z;
velocity_angular << odom_output.twist.twist.angular.x, odom_output.twist.twist.angular.y,
odom_output.twist.twist.angular.z;

fuse_core::Matrix15d jacobian;

fuse_core::Vector3d acceleration_linear;
if (params_.predict_with_acceleration)
{
acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y,
acceleration_output.accel.accel.linear.z;
}

::fuse_models::predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position,
orientation, velocity_linear, velocity_angular, acceleration_linear, jacobian);

// Convert back to tf2 representation
pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z()));
pose.setRotation(tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()));

odom_output.pose.pose.position.x = position.x();
odom_output.pose.pose.position.y = position.y();
odom_output.pose.pose.position.z = position.z();
odom_output.pose.pose.orientation.x = orientation.x();
odom_output.pose.pose.orientation.y = orientation.y();
odom_output.pose.pose.orientation.z = orientation.z();
odom_output.pose.pose.orientation.w = orientation.w();

odom_output.twist.twist.linear.x = velocity_linear.x();
odom_output.twist.twist.linear.y = velocity_linear.y();
odom_output.twist.twist.linear.z = velocity_linear.z();
odom_output.twist.twist.angular.x = velocity_angular.x();
odom_output.twist.twist.angular.y = velocity_angular.y();
odom_output.twist.twist.angular.z = velocity_angular.z();

if (params_.predict_with_acceleration)
{
acceleration_output.accel.accel.linear.x = acceleration_linear.x();
acceleration_output.accel.accel.linear.y = acceleration_linear.y();
acceleration_output.accel.accel.linear.z = acceleration_linear.z();
}

odom_output.header.stamp = to_predict_to;
acceleration_output.header.stamp = to_predict_to;

// Either the last covariance computation was skipped because there was no subscriber,
// or it failed
if (latest_covariance_valid)
{
fuse_core::Matrix15d covariance;
covariance.setZero();
Eigen::Map<fuse_core::Matrix6d> pose_covariance(odom_output.pose.covariance.data());
Eigen::Map<fuse_core::Matrix6d> twist_covariance(odom_output.twist.covariance.data());
Eigen::Map<fuse_core::Matrix3d> acceleration_covariance(acceleration_output.accel.covariance.data());

covariance.block<6, 6>(0, 0) = pose_covariance;
covariance.block<6, 6>(6, 6) = twist_covariance;
covariance.block<3, 3>(12, 12) = acceleration_covariance;

covariance = jacobian * covariance * jacobian.transpose();

auto process_noise_covariance = params_.process_noise_covariance;
if (params_.scale_process_noise)
{
common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, velocity_angular,
params_.velocity_linear_norm_min_, params_.velocity_angular_norm_min_);
}

covariance.noalias() += dt * process_noise_covariance;

pose_covariance = covariance.block<6, 6>(0, 0);
twist_covariance = covariance.block<6, 6>(6, 6);
acceleration_covariance = covariance.block<3, 3>(12, 12);
}
}

void Odometry3DPublisher::publishTF(nav_msgs::msg::Odometry const& odom_output, tf2::Transform& pose)
{
auto frame_id = odom_output.header.frame_id;
auto child_frame_id = odom_output.child_frame_id;

if (params_.invert_tf)
{
pose = pose.inverse();
std::swap(frame_id, child_frame_id);
}

geometry_msgs::msg::TransformStamped trans;
trans.header.stamp = odom_output.header.stamp;
trans.header.frame_id = frame_id;
trans.child_frame_id = child_frame_id;
trans.transform = tf2::toMsg(pose);
if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id)
{
try
{
auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id,
trans.header.stamp, params_.tf_timeout);

geometry_msgs::msg::TransformStamped map_to_odom;
tf2::doTransform(base_to_odom, map_to_odom, trans);
map_to_odom.child_frame_id = params_.odom_frame_id;
trans = map_to_odom;
}
catch (const std::exception& e)
{
RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000,
"Could not lookup the " << params_.base_link_frame_id << "->" << params_.odom_frame_id
<< " transform. Error: " << e.what());

return;
}
}

tf_broadcaster_->sendTransform(trans);
}

void Odometry3DPublisher::publishTimerCallback()
{
rclcpp::Time latest_stamp;
rclcpp::Time latest_covariance_stamp;
bool latest_covariance_valid = false;
nav_msgs::msg::Odometry odom_output;
geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output;
{
std::lock_guard<std::mutex> const lock(mutex_);

latest_stamp = latest_stamp_;
latest_covariance_stamp = latest_covariance_stamp_;
latest_covariance_valid = latest_covariance_valid_;
odom_output = odom_output_;
acceleration_output = acceleration_output_;
}

nav_msgs::msg::Odometry odom_output_predict = odom_output;

if (0u == latest_stamp.nanoseconds())
{
RCLCPP_WARN_STREAM_EXPRESSION(logger_, delayed_throttle_filter_.isEnabled(),
Expand All @@ -447,143 +593,32 @@ void Odometry3DPublisher::publishTimerCallback()
tf2::Transform pose;
tf2::fromMsg(odom_output.pose.pose, pose);

tf2::Transform pose_predict;
tf2::fromMsg(odom_output_predict.pose.pose, pose);

// If requested, we need to project our state forward in time using the 3D kinematic model
if (params_.predict_to_current_time)
{
rclcpp::Time const timer_now = interfaces_.get_node_clock_interface()->get_clock()->now();

// Convert pose in Eigen representation
fuse_core::Vector3d position;
fuse_core::Vector3d velocity_linear;
fuse_core::Vector3d velocity_angular;
Eigen::Quaterniond orientation;
position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z();
orientation.x() = pose.getRotation().x();
orientation.y() = pose.getRotation().y();
orientation.z() = pose.getRotation().z();
orientation.w() = pose.getRotation().w();
velocity_linear << odom_output.twist.twist.linear.x, odom_output.twist.twist.linear.y,
odom_output.twist.twist.linear.z;
velocity_angular << odom_output.twist.twist.angular.x, odom_output.twist.twist.angular.y,
odom_output.twist.twist.angular.z;

const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds();

fuse_core::Matrix15d jacobian;

fuse_core::Vector3d acceleration_linear;
if (params_.predict_with_acceleration)
{
acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y,
acceleration_output.accel.accel.linear.z;
}

predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position, orientation,
velocity_linear, velocity_angular, acceleration_linear, jacobian);

// Convert back to tf2 representation
pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z()));
pose.setRotation(tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()));

odom_output.pose.pose.position.x = position.x();
odom_output.pose.pose.position.y = position.y();
odom_output.pose.pose.position.z = position.z();
odom_output.pose.pose.orientation.x = orientation.x();
odom_output.pose.pose.orientation.y = orientation.y();
odom_output.pose.pose.orientation.z = orientation.z();
odom_output.pose.pose.orientation.w = orientation.w();

odom_output.twist.twist.linear.x = velocity_linear.x();
odom_output.twist.twist.linear.y = velocity_linear.y();
odom_output.twist.twist.linear.z = velocity_linear.z();
odom_output.twist.twist.angular.x = velocity_angular.x();
odom_output.twist.twist.angular.y = velocity_angular.y();
odom_output.twist.twist.angular.z = velocity_angular.z();

if (params_.predict_with_acceleration)
{
acceleration_output.accel.accel.linear.x = acceleration_linear.x();
acceleration_output.accel.accel.linear.y = acceleration_linear.y();
acceleration_output.accel.accel.linear.z = acceleration_linear.z();
}

odom_output.header.stamp = timer_now;
acceleration_output.header.stamp = timer_now;

// Either the last covariance computation was skipped because there was no subscriber,
// or it failed
if (latest_covariance_valid)
{
fuse_core::Matrix15d covariance;
covariance.setZero();
Eigen::Map<fuse_core::Matrix6d> pose_covariance(odom_output.pose.covariance.data());
Eigen::Map<fuse_core::Matrix6d> twist_covariance(odom_output.twist.covariance.data());
Eigen::Map<fuse_core::Matrix3d> acceleration_covariance(acceleration_output.accel.covariance.data());

covariance.block<6, 6>(0, 0) = pose_covariance;
covariance.block<6, 6>(6, 6) = twist_covariance;
covariance.block<3, 3>(12, 12) = acceleration_covariance;

covariance = jacobian * covariance * jacobian.transpose();

auto process_noise_covariance = params_.process_noise_covariance;
if (params_.scale_process_noise)
{
common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, velocity_angular,
params_.velocity_linear_norm_min_, params_.velocity_angular_norm_min_);
}

covariance.noalias() += dt * process_noise_covariance;

pose_covariance = covariance.block<6, 6>(0, 0);
twist_covariance = covariance.block<6, 6>(6, 6);
acceleration_covariance = covariance.block<3, 3>(12, 12);
}
predict(pose, odom_output, timer_now, acceleration_output, latest_covariance_valid);
}

odom_pub_->publish(odom_output);
acceleration_pub_->publish(acceleration_output);

if (params_.publish_tf)
{
auto frame_id = odom_output.header.frame_id;
auto child_frame_id = odom_output.child_frame_id;

if (params_.invert_tf)
if (params_.predict_to_future)
{
pose = pose.inverse();
std::swap(frame_id, child_frame_id);
}

geometry_msgs::msg::TransformStamped trans;
trans.header.stamp = odom_output.header.stamp;
trans.header.frame_id = frame_id;
trans.child_frame_id = child_frame_id;
trans.transform = tf2::toMsg(pose);
if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id)
{
try
{
auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id,
trans.header.stamp, params_.tf_timeout);

geometry_msgs::msg::TransformStamped map_to_odom;
tf2::doTransform(base_to_odom, map_to_odom, trans);
map_to_odom.child_frame_id = params_.odom_frame_id;
trans = map_to_odom;
}
catch (const std::exception& e)
rclcpp::Time predict_time;
{
RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000,
"Could not lookup the " << params_.base_link_frame_id << "->"
<< params_.odom_frame_id
<< " transform. Error: " << e.what());

return;
std::lock_guard<std::mutex> const lock(predict_timestamp_mutex_);
predict_time = predict_timestamp_;
}
predict(pose_predict, odom_output_predict, predict_time, acceleration_output, latest_covariance_valid);
publishTF(odom_output_predict, pose_predict);
}

tf_broadcaster_->sendTransform(trans);
publishTF(odom_output, pose);
}
}

Expand Down
1 change: 1 addition & 0 deletions fuse_tutorials/config/fuse_apriltag_tutorial.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,3 +53,4 @@ state_estimator:
world_frame_id: 'odom'
publish_tf: true
publish_frequency: 100.0
predict_to_future: true
Loading

0 comments on commit 6c6f9aa

Please sign in to comment.