From 971fbc50f2c46e090ecff31b0cd6d847a24f0da1 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 6 Mar 2025 14:13:08 -0700 Subject: [PATCH 1/6] wip --- .../parameters/transform_sensor_params.hpp | 7 ++-- .../include/fuse_models/transform_sensor.hpp | 3 +- fuse_models/src/transform_sensor.cpp | 40 ++++++++++++++----- 3 files changed, 36 insertions(+), 14 deletions(-) diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index 33917ef1c..7521c0e58 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -83,8 +83,8 @@ struct TransformSensorParams : public ParameterBase target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); - estimation_frame = - fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "estimation_frame"), estimation_frame); + estimation_frames = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "estimation_frames"), estimation_frames); pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); pose_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_covariance"), @@ -101,7 +101,8 @@ struct TransformSensorParams : public ParameterBase int queue_size{ 10 }; std::vector transforms; std::string target_frame; - std::string estimation_frame; + std::string base_frame; + std::vector estimation_frames; std::vector position_indices; std::vector orientation_indices; fuse_core::Loss::SharedPtr pose_loss; diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index 4bcbcccfa..8f9cab428 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -142,7 +142,8 @@ class TransformSensor : public fuse_core::AsyncSensorModel std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - std::set transforms_of_interest_; + std::set fiducial_transforms_; + std::set estimation_transforms_; rclcpp::Subscription::SharedPtr sub_; diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 7e36ddad0..02bb30168 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -102,12 +102,16 @@ void TransformSensor::onInit() for (auto const& name : params_.transforms) { - transforms_of_interest_.insert(name); + fiducial_transforms_.insert(name); } - if (params_.estimation_frame.empty()) + if (params_.estimation_frames.empty()) { - throw std::runtime_error("No estimation frame specified."); + throw std::runtime_error("No estimation frames specified."); + } + + for (auto const& name: params_.estimation_frames) { + estimation_transforms_.insert(name); } tf_buffer_ = std::make_unique(clock_); @@ -137,7 +141,8 @@ void TransformSensor::process(MessageType const& msg) std::string const& parent_tf_name = transform.header.frame_id; std::string const& child_tf_name = transform.child_frame_id; - bool const child_of_interest = transforms_of_interest_.find(child_tf_name) != transforms_of_interest_.end(); + bool const child_of_interest = fiducial_transforms_.find(child_tf_name) != fiducial_transforms_.end(); + bool const parent_of_interest = estimation_transforms_.find(parent_tf_name) != estimation_transforms_.end(); if (!child_of_interest) { @@ -149,7 +154,7 @@ void TransformSensor::process(MessageType const& msg) // we must either have april tag -> estimation frame or estimation frame -> april tag tf if (child_of_interest) { - if (parent_tf_name != params_.estimation_frame) + if (!parent_of_interest) { // we don't care about this transform , skip it RCLCPP_DEBUG(logger_, "Ignoring transform from %s to %s", transform.header.frame_id.c_str(), @@ -165,10 +170,9 @@ void TransformSensor::process(MessageType const& msg) tf2::Transform net_transform; tf2::fromMsg(transform.transform, net_transform); - std::string target_frame_name; if (!params_.target_frame.empty()) { - target_frame_name = params_.target_frame + "_" + child_tf_name; + std::string target_frame_name = params_.target_frame + "_" + child_tf_name; tf2::Transform april_to_target; try { @@ -187,9 +191,25 @@ void TransformSensor::process(MessageType const& msg) } net_transform = net_transform * april_to_target.inverse(); } - else - { - target_frame_name = ""; + // transform to base frame if it is defined + if (!params_.base_frame.empty()) { + tf2::Transform base_to_estimation; + try + { + tf2::fromMsg((*tf_buffer_) + .lookupTransform(parent_tf_name, params_.base_frame, + rclcpp::Time(transform.header.stamp.sec - 1, transform.header.stamp.nanosec), + params_.tf_timeout) + .transform, + base_to_estimation); + } + catch (...) + { + // tf2 throws a bunch of different exceptions that don't inherit from one base, just skip (this will happen for + // at least 1 second on startup) + continue; + } + net_transform = net_transform * base_to_estimation.inverse(); } // Create the pose from the transform From 5d2a83293d3b1c6754a32b175263f84a0a9db349 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 12 Mar 2025 15:47:50 -0600 Subject: [PATCH 2/6] fix default param --- .../fuse_models/parameters/odometry_3d_publisher_params.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 6d1f922b9..03a5e67fa 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -152,7 +152,7 @@ struct Odometry3DPublisherParams : public ParameterBase rclcpp::Duration covariance_throttle_period{ 0, 0 }; //!< The throttle period duration in seconds //!< to compute the covariance rclcpp::Duration tf_cache_time{ 10, 0 }; - rclcpp::Duration tf_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; + rclcpp::Duration tf_timeout{ 0, 0 }; int queue_size{ 1 }; std::string map_frame_id{ "map" }; std::string odom_frame_id{ "odom" }; From 6e65d992061620c0e7971f6d8cdfe64736f4f8ca Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 12 Mar 2025 15:48:17 -0600 Subject: [PATCH 3/6] add base frame param init --- .../include/fuse_models/parameters/transform_sensor_params.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index 7521c0e58..b1dcc4ef5 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -83,6 +83,8 @@ struct TransformSensorParams : public ParameterBase target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); + base_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "base_frame"), base_frame); + estimation_frames = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "estimation_frames"), estimation_frames); From 6db49fc747a2ba5f3ea6cacce8bec7e9712fc67c Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 12 Mar 2025 21:31:13 -0600 Subject: [PATCH 4/6] allow transform sensor to be used with base frame --- .../include/fuse_models/transform_sensor.hpp | 4 +- fuse_models/src/odometry_3d_publisher.cpp | 2 +- fuse_models/src/transform_sensor.cpp | 37 +++++++++++-------- 3 files changed, 24 insertions(+), 19 deletions(-) diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index 8f9cab428..dff9bea14 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -142,8 +142,8 @@ class TransformSensor : public fuse_core::AsyncSensorModel std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - std::set fiducial_transforms_; - std::set estimation_transforms_; + std::set fiducial_frames_; + std::set estimation_frames_; rclcpp::Subscription::SharedPtr sub_; diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 3b322b406..e473e0e79 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -441,7 +441,7 @@ void Odometry3DPublisher::predict(tf2::Transform& pose, nav_msgs::msg::Odometry& geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output, bool latest_covariance_valid) const { - double const dt = to_predict_to.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); + double const dt = std::min(to_predict_to.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(), 0.); // Convert pose in Eigen representation fuse_core::Vector3d position; fuse_core::Vector3d velocity_linear; diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 02bb30168..dc6b1e770 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -48,6 +48,8 @@ #include #include #include +#include +#include #include // Register this sensor model with ROS as a plugin. @@ -102,7 +104,7 @@ void TransformSensor::onInit() for (auto const& name : params_.transforms) { - fiducial_transforms_.insert(name); + fiducial_frames_.insert(name); } if (params_.estimation_frames.empty()) @@ -110,8 +112,9 @@ void TransformSensor::onInit() throw std::runtime_error("No estimation frames specified."); } - for (auto const& name: params_.estimation_frames) { - estimation_transforms_.insert(name); + for (auto const& name : params_.estimation_frames) + { + estimation_frames_.insert(name); } tf_buffer_ = std::make_unique(clock_); @@ -141,8 +144,8 @@ void TransformSensor::process(MessageType const& msg) std::string const& parent_tf_name = transform.header.frame_id; std::string const& child_tf_name = transform.child_frame_id; - bool const child_of_interest = fiducial_transforms_.find(child_tf_name) != fiducial_transforms_.end(); - bool const parent_of_interest = estimation_transforms_.find(parent_tf_name) != estimation_transforms_.end(); + bool const child_of_interest = fiducial_frames_.find(child_tf_name) != fiducial_frames_.end(); + bool const parent_of_interest = estimation_frames_.find(parent_tf_name) != estimation_frames_.end(); if (!child_of_interest) { @@ -191,17 +194,25 @@ void TransformSensor::process(MessageType const& msg) } net_transform = net_transform * april_to_target.inverse(); } + + // Create the pose from the transform + // we want a measurement from the april tag (transform of interest) to some reference frame + // if we have the opposite, invert it and use that + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header = transform.header; + pose.header.frame_id = parent_tf_name; // transform to base frame if it is defined - if (!params_.base_frame.empty()) { + if (!params_.base_frame.empty()) + { tf2::Transform base_to_estimation; try { tf2::fromMsg((*tf_buffer_) - .lookupTransform(parent_tf_name, params_.base_frame, + .lookupTransform(params_.base_frame, parent_tf_name, rclcpp::Time(transform.header.stamp.sec - 1, transform.header.stamp.nanosec), params_.tf_timeout) .transform, - base_to_estimation); + base_to_estimation); } catch (...) { @@ -209,15 +220,9 @@ void TransformSensor::process(MessageType const& msg) // at least 1 second on startup) continue; } - net_transform = net_transform * base_to_estimation.inverse(); + net_transform = base_to_estimation * net_transform; + pose.header.frame_id = params_.base_frame; } - - // Create the pose from the transform - // we want a measurement from the april tag (transform of interest) to some reference frame - // if we have the opposite, invert it and use that - geometry_msgs::msg::PoseWithCovarianceStamped pose; - pose.header = transform.header; - pose.header.frame_id = parent_tf_name; pose.pose.pose.orientation.w = net_transform.getRotation().w(); pose.pose.pose.orientation.x = net_transform.getRotation().x(); pose.pose.pose.orientation.y = net_transform.getRotation().y(); From 09a7be4f8b1c95a0ebadcbd02ca8e28e1144f96d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 13 Mar 2025 16:22:11 +0000 Subject: [PATCH 5/6] update documentation --- fuse_models/doc/sensor_models/transform_sensor.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/fuse_models/doc/sensor_models/transform_sensor.md b/fuse_models/doc/sensor_models/transform_sensor.md index 11c3efb9f..5c76e5416 100644 --- a/fuse_models/doc/sensor_models/transform_sensor.md +++ b/fuse_models/doc/sensor_models/transform_sensor.md @@ -8,7 +8,12 @@ This sensor is intended to be used to estimate the position of a body relative t For example, this sensor is useful in the case of measuring april tags on some external body and using those measurements to estimate the full pose and twist of that body. An example of this application can be seen [here](../../../fuse_tutorials/config/fuse_apriltag_tutorial.yaml) (or try `ros2 launch fuse_tutorials fuse_apriltag_tutorial.launch.py` to see the tutorial in action). -More generally, this sensor takes a transform between some set `estimation_frame` and one of a predefined set of `transforms`. When it receives such a transform, it will then apply a transform to this measurement to change it to be a measurement of the given `target_frame`. Then, it generates a 3D pose transaction for use by its parent optimizer. +More generally, this sensor takes a transform between one frame in the set `estimation_frames` and one transform from the set `transforms`. When it receives such a transform, it will then apply a transform to this measurement to change it to be a measurement of the given `target_frame`. Then, it generates a 3D pose transaction for use by its parent optimizer. + +If you want to use multiple `estimation_frames`, you should define a `base_frame`. +If this frame is non-empty, then you should set the `map_frame_id` and `world_frame_id` in the publisher to the same value. +When this is set, the sensor will transform its pose to be a measurement in `base_frame` of `target_frame`, and the odom it produces will be in `base_frame`. This can be used when e.g. you have multiple cameras, all positioned relative to `world`, and want them measuring the same object. +This allows the transform sensor to use any number of cameras to estimate the pose of some other object. A slightly confusing aspect of the sensor is the need for multiple definitions of the target frame. Every frame in `transforms` needs a corresponding `target_frame` to be published for it to be used. This is simply because `tf` uses a tree data structure, and the target frames are leaves. These should all end up being in the same global location (discounting measurement noise) but will have different names. From 12c2040cdb6e7c2e1895f036d4bdcebe90abb36a Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 18 Mar 2025 14:34:19 -0600 Subject: [PATCH 6/6] switch to vec of array --- .../include/fuse_models/transform_sensor.hpp | 1 + fuse_models/src/transform_sensor.cpp | 23 ++++++++++++++++--- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index dff9bea14..09e25fb6e 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -144,6 +144,7 @@ class TransformSensor : public fuse_core::AsyncSensorModel std::unique_ptr tf_listener_; std::set fiducial_frames_; std::set estimation_frames_; + std::vector> pose_covariances_; rclcpp::Subscription::SharedPtr sub_; diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index dc6b1e770..7edceaabb 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -117,6 +117,21 @@ void TransformSensor::onInit() estimation_frames_.insert(name); } + if (params_.pose_covariance.size() != 6 * estimation_frames_.size()) + { + throw std::runtime_error("Must provide 6 `pose_covariance` values per estimation frame"); + } + + for (std::size_t i = 0; i < estimation_frames_.size(); ++i) + { + pose_covariances_.emplace_back(); + auto& cur_entry = pose_covariances_.back(); + for (std::size_t j = 0; j < 6; ++j) + { + cur_entry[j] = params_.pose_covariance[(i * 6) + j]; + } + } + tf_buffer_ = std::make_unique(clock_); tf_listener_ = std::make_unique(*tf_buffer_, &interfaces_); } @@ -145,7 +160,8 @@ void TransformSensor::process(MessageType const& msg) std::string const& child_tf_name = transform.child_frame_id; bool const child_of_interest = fiducial_frames_.find(child_tf_name) != fiducial_frames_.end(); - bool const parent_of_interest = estimation_frames_.find(parent_tf_name) != estimation_frames_.end(); + auto const parent_it = estimation_frames_.find(parent_tf_name); + bool const parent_of_interest = parent_it != estimation_frames_.end(); if (!child_of_interest) { @@ -165,6 +181,7 @@ void TransformSensor::process(MessageType const& msg) continue; } } + std::size_t estimation_index = std::distance(estimation_frames_.begin(), parent_it); RCLCPP_DEBUG(logger_, "Got transform of interest from %s to %s", transform.header.frame_id.c_str(), child_tf_name.c_str()); // Create a transaction object @@ -232,9 +249,9 @@ void TransformSensor::process(MessageType const& msg) pose.pose.pose.position.z = net_transform.getOrigin().z(); // TODO(henrygerardmoore): figure out better method to set the covariance - for (std::size_t i = 0; i < params_.pose_covariance.size(); ++i) + for (std::size_t i = 0; i < pose_covariances_[estimation_index].size(); ++i) { - pose.pose.covariance[i * 7] = params_.pose_covariance[i]; + pose.pose.covariance[i * 7] = pose_covariances_[estimation_index][i]; } bool const validate = !params_.disable_checks;