Optimizer changes

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

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>
 

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

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

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_;

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;
 };