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. 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" }; 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..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,8 +83,10 @@ 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); + 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); 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 +103,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..09e25fb6e 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -142,7 +142,9 @@ class TransformSensor : public fuse_core::AsyncSensorModel std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - std::set transforms_of_interest_; + std::set fiducial_frames_; + std::set estimation_frames_; + std::vector> pose_covariances_; 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 7e36ddad0..7edceaabb 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,12 +104,32 @@ void TransformSensor::onInit() for (auto const& name : params_.transforms) { - transforms_of_interest_.insert(name); + fiducial_frames_.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_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_); @@ -137,7 +159,9 @@ 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_frames_.find(child_tf_name) != fiducial_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) { @@ -149,7 +173,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(), @@ -157,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 @@ -165,10 +190,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,10 +211,6 @@ void TransformSensor::process(MessageType const& msg) } net_transform = net_transform * april_to_target.inverse(); } - else - { - target_frame_name = ""; - } // Create the pose from the transform // we want a measurement from the april tag (transform of interest) to some reference frame @@ -198,6 +218,28 @@ void TransformSensor::process(MessageType const& msg) 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()) + { + tf2::Transform base_to_estimation; + try + { + tf2::fromMsg((*tf_buffer_) + .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); + } + 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 = base_to_estimation * net_transform; + pose.header.frame_id = params_.base_frame; + } 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(); @@ -207,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;