Optimizer changes (#10)

* fix clang tidy errors in imu_3d * clang tidy fixes for various core files * more clang tidy fixes, allow c-style arrays * remove unused headers * add skeleton of april tag pose detector, clang tidy fix to imu * switch copyright * clang tidy fixes * undo unique ptr changes * april tag WIP * clang tidy fixes * build fixes * clang-tidy fixes * clang-tidy fixes * add skeleton of apriltag tutorial * remove now-unused compile options * finish april tag simulator * clang tidy fixes * remove unknown compile warning * fix memory leak * applied PR feedback * remove extraneous sleep * use more visually pleasing parameters * improve the tutorial config file * fix bit shift logic :woozy: * resolve TODO * clangd fixes * readme update * clang tidy fixes * remove unused imports * merge main * clang tidy fixes * add constructors * clang tidy fixes * remove unused includes * clang tidy const correctness * fixes in batch optimizer * clang tidy fixes * add on-demand prediction * got prediction fully working, added to config
PickNikRobotics · Dec 9, 2024 · e46c7b3 · e46c7b3
1 parent ef94df5
commit e46c7b3
Show file tree

Hide file tree

Showing 17 changed files with 504 additions and 189 deletions.
diff --git a/README.md b/README.md
@@ -1,14 +1,16 @@
 # fuse
 
-**NOTE**: The `rolling` branch is a [work in progress](https://github.com/locusrobotics/fuse/issues/276) port of the fuse stack to ROS 2, it is **not** expected to work until said work is done!
+Welcome to PickNik Robotics's fork of fuse!
 
-The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications
-include state estimation, localization, mapping, and calibration.
+## Getting Started
+
+Using the Dockerfile is the easiest way to get started. Once inside, simply `rosdep update` and `colcon build` and you're ready to get started! Try `ros2 launch fuse_tutorials fuse_3d_tutorial.launch.py` and look at its pertinent code for a crash course in fuse and its capabilities.
 
 ## Overview
 
-fuse is a ROS framework for performing sensor fusion using nonlinear least squares optimization techniques. In
-particular, fuse provides:
+The fuse stack provides a general architecture for performing sensor fusion live on a robot using nonlinear least squares optimization techniques.
+Some possible applications include state estimation, localization, mapping, and calibration.
+In particular, fuse provides:
 
 * a plugin-based system for modeling sensor measurements
 * a similar plugin-based system for motion models

diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp
@@ -36,10 +36,7 @@
 
 #include <ceres/cost_function.h>
 
-#include <algorithm>
 #include <cassert>
-#include <iterator>
-#include <ostream>
 #include <string>
 #include <vector>
 

diff --git a/fuse_core/include/fuse_core/fuse_macros.hpp b/fuse_core/include/fuse_core/fuse_macros.hpp
@@ -58,10 +58,6 @@
 // Required by __MAKE_SHARED_ALIGNED_DEFINITION, that uses Eigen::aligned_allocator<T>().
 #include <Eigen/Core>
 
-#include <memory>
-#include <string>
-#include <utility>
-
 /**
  * Creates a custom new() implementation that ensures memory is allocated with proper byte
  * alignment. This should be added to the public section of classes or structs that contain

diff --git a/fuse_core/include/fuse_core/transaction.hpp b/fuse_core/include/fuse_core/transaction.hpp
@@ -111,7 +111,7 @@ class Transaction
   /**
    * @brief Read-only access to this transaction's timestamp
    */
-  const rclcpp::Time& stamp() const
+  [[nodiscard]] const rclcpp::Time& stamp() const
   {
     return stamp_;
   }
@@ -129,7 +129,7 @@ class Transaction
    *
    * @return An iterator range containing all involved timestamps, ordered oldest to newest
    */
-  const_stamp_range involvedStamps() const
+  [[nodiscard]] const_stamp_range involvedStamps() const
   {
     return involved_stamps_;
   }
@@ -140,29 +140,29 @@ class Transaction
    *
    * @return The minimum (oldest) timestamp.
    */
-  const rclcpp::Time& minStamp() const;
+  [[nodiscard]] const rclcpp::Time& minStamp() const;
 
   /**
    * @brief Read-only access to the maximum (newest) timestamp among the transaction's stamp and all
    *        involved timestamps, if any
    *
    * @return The maximum (newest) timestamp.
    */
-  const rclcpp::Time& maxStamp() const;
+  [[nodiscard]] const rclcpp::Time& maxStamp() const;
 
   /**
    * @brief Read-only access to the added constraints
    *
    * @return  An iterator range containing all added constraints
    */
-  const_constraint_range addedConstraints() const;
+  [[nodiscard]] const_constraint_range addedConstraints() const;
 
   /**
    * @brief Read-only access to the removed constraints
    *
    * @return  An iterator range containing all removed constraint UUIDs
    */
-  const_uuid_range removedConstraints() const
+  [[nodiscard]] const_uuid_range removedConstraints() const
   {
     return removed_constraints_;
   }
@@ -172,14 +172,14 @@ class Transaction
    *
    * @return  An iterator range containing all added variables
    */
-  const_variable_range addedVariables() const;
+  [[nodiscard]] const_variable_range addedVariables() const;
 
   /**
    * @brief Read-only access to the removed variables
    *
    * @return  An iterator range containing all removed variable UUIDs
    */
-  const_uuid_range removedVariables() const
+  [[nodiscard]] const_uuid_range removedVariables() const
   {
     return removed_variables_;
   }
@@ -190,7 +190,7 @@ class Transaction
    *
    * @return  True if the transaction is empty, false otherwise
    */
-  bool empty() const;
+  [[nodiscard]] bool empty() const;
 
   /**
    * @brief Add a timestamp to the "involved stamps" collection
@@ -275,7 +275,7 @@ class Transaction
    *
    * @return A unique pointer to a new instance of the most-derived Variable
    */
-  Transaction::UniquePtr clone() const;
+  [[nodiscard]] Transaction::UniquePtr clone() const;
 
   /**
    * @brief Serialize this Constraint into the provided binary archive

diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp
@@ -118,6 +118,10 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher
    * @brief Destructor
    */
   virtual ~Odometry3DPublisher() = default;
+  Odometry3DPublisher(Odometry3DPublisher const&) = delete;
+  Odometry3DPublisher(Odometry3DPublisher&&) = delete;
+  Odometry3DPublisher& operator=(Odometry3DPublisher const&) = delete;
+  Odometry3DPublisher& operator=(Odometry3DPublisher&&) = delete;
 
   /**
    * @brief Shadowing extension to the AsyncPublisher::initialize call
@@ -126,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.
@@ -216,7 +226,11 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_predict_;
   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_;

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
@@ -36,10 +36,8 @@
 
 #include <ceres/covariance.h>
 
-#include <algorithm>
 #include <cassert>
 #include <string>
-#include <vector>
 
 #include <fuse_models/parameters/parameter_base.hpp>
 
@@ -77,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(
@@ -158,6 +160,9 @@ 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 predict_topic{ "predicted_odometry/filtered" };
   std::string acceleration_topic{ "acceleration/filtered" };
   ceres::Covariance::Options covariance_options;
 };