From bb77ad71636f875889d2db43d5993cc31616e440 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Sun, 20 Nov 2022 16:49:07 -0500 Subject: [PATCH 01/17] Implement check for adding constraints to marginalized variables --- fuse_optimizers/src/fixed_lag_smoother.cpp | 24 ++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a91850825..1ea2d36b0 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -197,6 +197,30 @@ void FixedLagSmoother::optimizationLoop() { continue; } + // check if new transaction has added constraints to variables that are to be marginalized + std::vector faulty_constraints; + for(auto& c: new_transaction->addedConstraints()) + { + for(auto var_uuid: c.variables()) + { + for (auto marginal_uuid : marginal_transaction_.removedVariables()) + { + if (var_uuid == marginal_uuid) + { + faulty_constraints.push_back(c.uuid()); + break; + } + } + } + } + if(faulty_constraints.size() > 0) + { + ROS_WARN_STREAM("Removing invalid constraints."); + for(auto& faulty_constraint: faulty_constraints) + { + new_transaction->removeConstraint(faulty_constraint); + } + } // Prepare for selecting the marginal variables preprocessMarginalization(*new_transaction); // Combine the new transactions with any marginal transaction from the end of the last cycle From 6a0ea4e4e659ca67c1831caf0ec2d78350fa2ffa Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Tue, 17 Jan 2023 19:45:07 -0500 Subject: [PATCH 02/17] Add helper to landmakr variable --- fuse_optimizers/CMakeLists.txt | 1 + .../fuse_variables/point_3d_landmark.h | 197 +++++++++--------- 2 files changed, 102 insertions(+), 96 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 798787394..1c101fd6b 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") set(build_depends diagnostic_updater diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.h b/fuse_variables/include/fuse_variables/point_3d_landmark.h index 1661c11a1..598079695 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.h @@ -49,107 +49,112 @@ namespace fuse_variables { -/** - * @brief Variable representing a 3D point landmark that exists across time. - * - * This is commonly used to represent locations of visual features. The UUID of this class is constant after - * construction and dependent on a user input database id. As such, the database id cannot be altered after - * construction. - */ -class Point3DLandmark : public FixedSizeVariable<3> -{ -public: - FUSE_VARIABLE_DEFINITIONS(Point3DLandmark); - - /** - * @brief Can be used to directly index variables in the data array - */ - enum : size_t - { - X = 0, - Y = 1, - Z = 2 - }; - - /** - * @brief Default constructor - */ - Point3DLandmark() = default; - /** - * @brief Construct a point 3D variable given a landmarks id + * @brief Variable representing a 3D point landmark that exists across time. * - * @param[in] landmark_id The id associated to a landmark - */ - explicit Point3DLandmark(const uint64_t& landmark_id); - - /** - * @brief Read-write access to the X-axis position. + * This is commonly used to represent locations of visual features. The UUID of this class is constant after + * construction and dependent on a user input database id. As such, the database id cannot be altered after + * construction. */ - double& x() { return data_[X]; } - - /** - * @brief Read-only access to the X-axis position. - */ - const double& x() const { return data_[X]; } - - /** - * @brief Read-write access to the Y-axis position. - */ - double& y() { return data_[Y]; } - - /** - * @brief Read-only access to the Y-axis position. - */ - const double& y() const { return data_[Y]; } - - /** - * @brief Read-write access to the Z-axis position. - */ - double& z() { return data_[Z]; } - - /** - * @brief Read-only access to the Z-axis position. - */ - const double& z() const { return data_[Z]; } - - /** - * @brief Read-only access to the id - */ - const uint64_t& id() const { return id_; } - - /** - * @brief Print a human-readable description of the variable to the provided - * stream. - * - * @param[out] stream The stream to write to. Defaults to stdout. - */ - void print(std::ostream& stream = std::cout) const override; - -private: - // Allow Boost Serialization access to private methods - friend class boost::serialization::access; - uint64_t id_ { 0 }; - - /** - * @brief The Boost Serialize method that serializes all of the data members - * in to/out of the archive - * - * @param[in/out] archive - The archive object that holds the serialized class - * members - * @param[in] version - The version of the archive being read/written. - * Generally unused. - */ - template - void serialize(Archive& archive, const unsigned int /* version */) + class Point3DLandmark : public FixedSizeVariable<3> { - archive& boost::serialization::base_object>(*this); - archive& id_; - } -}; + public: + FUSE_VARIABLE_DEFINITIONS(Point3DLandmark); + + /** + * @brief Can be used to directly index variables in the data array + */ + enum : size_t + { + X = 0, + Y = 1, + Z = 2 + }; + + /** + * @brief Default constructor + */ + Point3DLandmark() = default; + + /** + * @brief Construct a point 3D variable given a landmarks id + * + * @param[in] landmark_id The id associated to a landmark + */ + explicit Point3DLandmark(const uint64_t &landmark_id); + + /** + * @brief Read-write access to the X-axis position. + */ + double &x() { return data_[X]; } + + /** + * @brief Read-only access to the X-axis position. + */ + const double &x() const { return data_[X]; } + + /** + * @brief Read-write access to the Y-axis position. + */ + double &y() { return data_[Y]; } + + /** + * @brief Read-only access to the Y-axis position. + */ + const double &y() const { return data_[Y]; } + + /** + * @brief Read-write access to the Z-axis position. + */ + double &z() { return data_[Z]; } + + /** + * @brief Read-only access to the Z-axis position. + */ + const double &z() const { return data_[Z]; } + + /** + * @brief Read-only access to the id + */ + const uint64_t &id() const { return id_; } + + /** + * @brief Access to the point as a vector + */ + Eigen::Vector3d point() const { return Eigen::Vector3d(x(), y(), z()); } + + /** + * @brief Print a human-readable description of the variable to the provided + * stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream &stream = std::cout) const override; + + private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + uint64_t id_{0}; + + /** + * @brief The Boost Serialize method that serializes all of the data members + * in to/out of the archive + * + * @param[in/out] archive - The archive object that holds the serialized class + * members + * @param[in] version - The version of the archive being read/written. + * Generally unused. + */ + template + void serialize(Archive &archive, const unsigned int /* version */) + { + archive &boost::serialization::base_object>(*this); + archive &id_; + } + }; -} // namespace fuse_variables +} // namespace fuse_variables BOOST_CLASS_EXPORT_KEY(fuse_variables::Point3DLandmark); -#endif // FUSE_VARIABLES_POINT_3D_LANDMARK_H +#endif // FUSE_VARIABLES_POINT_3D_LANDMARK_H From 477ee280f7db461dde78ace14bb7f41fc0eaac42 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 21 Nov 2021 19:48:35 -0800 Subject: [PATCH 03/17] WIP --- fuse_optimizers/CMakeLists.txt | 1 + .../fuse_optimizers/fixed_lag_smoother.h | 160 +---- .../fixed_lag_smoother_params.h | 49 +- .../fuse_optimizers/windowed_optimizer.h | 315 +++++++++ .../windowed_optimizer_params.h | 114 ++++ fuse_optimizers/src/fixed_lag_smoother.cpp | 609 +---------------- .../src/fixed_lag_smoother_node.cpp | 5 +- fuse_optimizers/src/windowed_optimizer.cpp | 613 ++++++++++++++++++ 8 files changed, 1089 insertions(+), 777 deletions(-) create mode 100644 fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h create mode 100644 fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h create mode 100644 fuse_optimizers/src/windowed_optimizer.cpp diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 1c101fd6b..e8a1fa34b 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -36,6 +36,7 @@ add_library(${PROJECT_NAME} src/batch_optimizer.cpp src/fixed_lag_smoother.cpp src/optimizer.cpp + src/windowed_optimizer.cpp src/variable_stamp_index.cpp ) add_dependencies(${PROJECT_NAME} diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index da7d56ad5..749d49a21 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include #include @@ -102,99 +102,43 @@ namespace fuse_optimizers * motion models to be generated. Once the timeout expires, that * transaction will be deleted from the queue. */ -class FixedLagSmoother : public Optimizer +class FixedLagSmoother : public WindowedOptimizer { public: SMART_PTR_DEFINITIONS(FixedLagSmoother); using ParameterType = FixedLagSmootherParams; + /// @todo(swilliams) I changed the constructor. This technically breaks API backwards compatibility. Think of a + /// better way to handle supplying the parameters. /** * @brief Constructor * * @param[in] graph The derived graph object. This allows different graph implementations to be used * with the same optimizer code. + * @param[in] params A structure containing all of the configuration parameters required by the + * fixed-lag smoother * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ FixedLagSmoother( fuse_core::Graph::UniquePtr graph, + const ParameterType& params, const ros::NodeHandle& node_handle = ros::NodeHandle(), const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); - /** - * @brief Destructor - */ - virtual ~FixedLagSmoother(); - protected: - /** - * Structure containing the information required to process a transaction after it was received. - */ - struct TransactionQueueElement - { - std::string sensor_name; - fuse_core::Transaction::SharedPtr transaction; - - const ros::Time& stamp() const { return transaction->stamp(); } - const ros::Time& minStamp() const { return transaction->minStamp(); } - const ros::Time& maxStamp() const { return transaction->maxStamp(); } - }; - - /** - * @brief Queue of Transaction objects, sorted by timestamp. - * - * Note: Because the queue size of the fixed-lag smoother is expected to be small, the sorted queue is implemented - * using a std::vector. The queue size must exceed several hundred entries before a std::set will outperform a - * sorted vector. - * - * Also, we sort the queue with the smallest stamp last. This allows us to clear the queue using the more - * efficient pop_back() operation. - */ - using TransactionQueue = std::vector; - // Read-only after construction - std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process ParameterType params_; //!< Configuration settings for this fixed-lag smoother - // Inherently thread-safe - std::atomic ignited_; //!< Flag indicating the optimizer has received a transaction from an ignition sensor - //!< and it is queued but not processed yet - std::atomic optimization_running_; //!< Flag indicating the optimization thread should be running - std::atomic started_; //!< Flag indicating the optimizer has received a transaction from an ignition sensor - - // Guarded by pending_transactions_mutex_ - std::mutex pending_transactions_mutex_; //!< Synchronize modification of the pending_transactions_ container - TransactionQueue pending_transactions_; //!< The set of received transactions that have not been added to the - //!< optimizer yet. Transactions are added by the main thread, and removed - //!< and processed by the optimization thread. - // Guarded by optimization_mutex_ - std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized - // fuse_core::Graph* graph_ member from the base class + /// @todo(swilliams) This should probably be guarded by its own mutex ros::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window - fuse_core::Transaction marginal_transaction_; //!< The marginals to add during the next optimization cycle VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable - ceres::Solver::Summary summary_; //!< Optimization summary, written by optimizationLoop and read by setDiagnostics - - // Guarded by optimization_requested_mutex_ - std::mutex optimization_requested_mutex_; //!< Required condition variable mutex - ros::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers a warning if exceeded. - bool optimization_request_; //!< Flag to trigger a new optimization - std::condition_variable optimization_requested_; //!< Condition variable used by the optimization thread to wait - //!< until a new optimization is requested by the main thread - - // Guarded by start_time_mutex_ - mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable - ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction - - // Ordering ROS objects with callbacks last - ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency - ros::ServiceServer reset_service_server_; //!< Service that resets the optimizer to its initial state /** - * @brief Automatically start the smoother if no ignition sensors are specified + * @brief Compute the oldest timestamp that is part of the configured lag window */ - void autostart(); + ros::Time computeLagExpirationTime() const; /** * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called @@ -206,12 +150,7 @@ class FixedLagSmoother : public Optimizer * * @param[in] new_transaction All new, non-marginal-related transactions that *will be* applied to the graph */ - void preprocessMarginalization(const fuse_core::Transaction& new_transaction); - - /** - * @brief Compute the oldest timestamp that is part of the configured lag window - */ - ros::Time computeLagExpirationTime() const; + void preprocessMarginalization(const fuse_core::Transaction& new_transaction) override; /** * @brief Compute the set of variables that should be marginalized from the graph @@ -222,7 +161,7 @@ class FixedLagSmoother : public Optimizer * @param[in] lag_expiration The oldest timestamp that should remain in the graph * @return A container with the set of variables to marginalize out. Order of the variables is not specified. */ - std::vector computeVariablesToMarginalize(const ros::Time& lag_expiration); + std::vector computeVariablesToMarginalize() override; /** * @brief Perform any required post-marginalization bookkeeping @@ -233,81 +172,12 @@ class FixedLagSmoother : public Optimizer * @param[in] marginal_transaction The actual changes to the graph caused my marginalizing out the requested * variables. */ - void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction); - - /** - * @brief Function that optimizes all constraints, designed to be run in a separate thread. - * - * This function waits for an optimization or shutdown signal, then either calls optimize() or exits appropriately. - */ - void optimizationLoop(); - - /** - * @brief Callback fired at a fixed frequency to trigger a new optimization cycle. - * - * This callback checks if a current optimization cycle is still running. If not, a new optimization cycle is started. - * If so, we simply wait for the next timer event to start another optimization cycle. - * - * @param event The ROS timer event metadata - */ - void optimizerTimerCallback(const ros::TimerEvent& event); - - /** - * @brief Generate motion model constraints for pending transactions and combine them into a single transaction - * - * Transactions are processed sequentially based on timestamp. If motion models are successfully generated for a - * pending transactions, that transaction is merged into the combined transaction and removed from the pending - * queue. If motion models fail to generate after the configured transaction_timeout_, the transaction will be - * deleted from the pending queue and a warning will be displayed. - * - * @param[out] transaction The transaction object to be augmented with pending motion model and sensor transactions - * @param[in] lag_expiration The oldest timestamp that should remain in the graph - */ - void processQueue(fuse_core::Transaction& transaction, const ros::Time& lag_expiration); - - /** - * @brief Service callback that resets the optimizer to its original state - */ - bool resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - - /** - * @brief Thread-safe read-only access to the optimizer start time - */ - ros::Time getStartTime() const - { - std::lock_guard lock(start_time_mutex_); - return start_time_; - } - - /** - * @brief Thread-safe write access to the optimizer start time - */ - void setStartTime(const ros::Time& start_time) - { - std::lock_guard lock(start_time_mutex_); - start_time_ = start_time; - } - - /** - * @brief Callback fired every time the SensorModel plugin creates a new transaction - * - * This callback is responsible for ensuring all associated motion models are applied before any other processing - * takes place. See Optimizer::applyMotionModels() for a helper function that does just that. - * - * This implementation shares ownership of the transaction object. - * - * @param[in] name The name of the sensor that produced the Transaction - * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin - */ - void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) override; /** - * @brief Update and publish diagnotics - * @param[in] status The diagnostic status + * @brief Perform any required operations whenever the optimizer is reset */ - void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override; + void onReset() override; }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h index a9c0972c1..a7c510e8d 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -52,7 +53,7 @@ namespace fuse_optimizers /** * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class */ -struct FixedLagSmootherParams +struct FixedLagSmootherParams : public WindowedOptimizerParams { public: /** @@ -60,33 +61,6 @@ struct FixedLagSmootherParams */ ros::Duration lag_duration { 5.0 }; - /** - * @brief The target duration for optimization cycles - * - * If an optimization takes longer than expected, an optimization cycle may be skipped. The optimization period - * may be specified in either the "optimization_period" parameter in seconds, or in the "optimization_frequency" - * parameter in Hz. - */ - ros::Duration optimization_period { 0.1 }; - - /** - * @brief The topic name of the advertised reset service - */ - std::string reset_service { "~reset" }; - - /** - * @brief The maximum time to wait for motion models to be generated for a received transaction. - * - * Transactions are processed sequentially, so no new transactions will be added to the graph while waiting for - * motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue. - */ - ros::Duration transaction_timeout { 0.1 }; - - /** - * @brief Ceres Solver::Options object that controls various aspects of the optimizer. - */ - ceres::Solver::Options solver_options; - /** * @brief Method for loading parameter values from ROS. * @@ -94,25 +68,10 @@ struct FixedLagSmootherParams */ void loadFromROS(const ros::NodeHandle& nh) { + WindowedOptimizerParams::loadFromROS(nh); + // Read settings from the parameter server fuse_core::getPositiveParam(nh, "lag_duration", lag_duration); - - if (nh.hasParam("optimization_frequency")) - { - double optimization_frequency{ 1.0 / optimization_period.toSec() }; - fuse_core::getPositiveParam(nh, "optimization_frequency", optimization_frequency); - optimization_period.fromSec(1.0 / optimization_frequency); - } - else - { - fuse_core::getPositiveParam(nh, "optimization_period", optimization_period); - } - - nh.getParam("reset_service", reset_service); - - fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout); - - fuse_core::loadSolverOptionsFromROS(ros::NodeHandle(nh, "solver_options"), solver_options); } }; diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h new file mode 100644 index 000000000..00a5c7f3e --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h @@ -0,0 +1,315 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_OPTIMIZERS_WINDOWED_OPTIMIZER_H +#define FUSE_OPTIMIZERS_WINDOWED_OPTIMIZER_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +namespace fuse_optimizers +{ + +/** + * @brief The WindowedOptimizer acts as a base class for a variety of optimizers that maintain a small window of active + * variables. + * + * The exact method for determining the set of variables to remove is delegated to the derived classes. This class + * handles the common set of interactions needed by any derived implementation. During optimization: + * (1) new variables and constraints are added to the graph + * (2) the augmented graph is optimized and the variable values are updated + * (3) all motion models, sensors, and publishers are notified of the updated graph + * (4) the variables to be removed, as defined by the derived classes, are marginalized out. + * + * Optimization is performed at a fixed frequency, controlled by the \p optimization_frequency parameter. Received + * sensor transactions are queued while the optimization is processing, then applied to the graph at the start of the + * next optimization cycle. If the previous optimization is not yet complete when the optimization period elapses, + * then a warning will be logged but a new optimization will *not* be started. The previous optimization will run to + * completion, and the next optimization will not begin until the next scheduled optimization period. + * + * Parameters: + * - motion_models (struct array) The set of motion model plugins to load + * @code{.yaml} + * - name: string (A unique name for this motion model) + * type: string (The plugin loader class string for the desired motion model type) + * - ... + * @endcode + * - optimization_frequency (float, default: 10.0) The target frequency for optimization cycles. If an optimization + * takes longer than expected, an optimization cycle may be skipped. + * - publishers (struct array) The set of publisher plugins to load + * @code{.yaml} + * - name: string (A unique name for this publisher) + * type: string (The plugin loader class string for the desired publisher type) + * - ... + * @endcode + * - sensor_models (struct array) The set of sensor model plugins to load + * @code{.yaml} + * - name: string (A unique name for this sensor model) + * type: string (The plugin loader class string for the desired sensor model type) + * motion_models: [name1, name2, ...] (An optional list of motion model names that should be applied) + * - ... + * @endcode + * - transaction_timeout (float, default: 0.10) The maximum time to wait for motion models to be generated for a + * received transactions. Transactions are processes sequentially, so + * no new transactions will be added to the graph while waiting for + * motion models to be generated. Once the timeout expires, that + * transaction will be deleted from the queue. + */ +class WindowedOptimizer : public Optimizer +{ +public: + SMART_PTR_DEFINITIONS(WindowedOptimizer); + using ParameterType = WindowedOptimizerParams; + + /// @todo(swilliams) What is going to happen is the derived class will make a DerivedParameters from the + /// WindowedOptimizerParams base class. It will then pass the derived params object to the WindowedOptimizer, which + /// will keep a copy of just the WindowedOptimizerParams portion of the parameters. This means that the derived + /// class will have two copies of the parameters; changing values in the derived class's copy will have no effect on + /// the base class copy. This is not exactly expected behavior. We could keep a pointer to the derived class params + /// instead, though that looks a bit ugly. I'm open to other suggestions. + /** + * @brief Constructor + * + * @param[in] graph - The derived graph object. This allows different graph implementations to be used with the + * same optimizer code. + * @param[in] params - A structure containing all of the configuration parameters required by the windowed optimizer + * @param[in] node_handle - A node handle in the global namespace + * @param[in] private_node_handle - A node handle in the node's private namespace + */ + WindowedOptimizer( + fuse_core::Graph::UniquePtr graph, + const ParameterType& params = ParameterType(), + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + + /** + * @brief Destructor + */ + virtual ~WindowedOptimizer(); + +protected: + /** + * Structure containing the information required to process a transaction after it was received. + */ + struct TransactionQueueElement + { + std::string sensor_name; + fuse_core::Transaction::SharedPtr transaction; + + const ros::Time& stamp() const { return transaction->stamp(); } + const ros::Time& minStamp() const { return transaction->minStamp(); } + const ros::Time& maxStamp() const { return transaction->maxStamp(); } + }; + + /** + * @brief Queue of Transaction objects, sorted by timestamp. + * + * Note: Because the queue size of the fixed-lag smoother is expected to be small, the sorted queue is implemented + * using a std::vector. The queue size must exceed several hundred entries before a std::set will outperform a + * sorted vector. + * + * Also, we sort the queue with the smallest stamp last. This allows us to clear the queue using the more + * efficient pop_back() operation. + */ + using TransactionQueue = std::vector; + + // Read-only after construction + std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process + ParameterType params_; //!< Configuration settings for this fixed-lag smoother + + // Inherently thread-safe + std::atomic ignited_; //!< Flag indicating the optimizer has received a transaction from an ignition sensor + //!< and it is queued but not processed yet + std::atomic optimization_running_; //!< Flag indicating the optimization thread should be running + std::atomic started_; //!< Flag indicating the optimizer has received a transaction from an ignition sensor + + // Guarded by pending_transactions_mutex_ + std::mutex pending_transactions_mutex_; //!< Synchronize modification of the pending_transactions_ container + TransactionQueue pending_transactions_; //!< The set of received transactions that have not been added to the + //!< optimizer yet. Transactions are added by the main thread, and removed + //!< and processed by the optimization thread. + + // Guarded by optimization_mutex_ + std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized + // fuse_core::Graph* graph_ member from the base class + fuse_core::Transaction marginal_transaction_; //!< The marginals to add during the next optimization cycle + ceres::Solver::Summary summary_; //!< Optimization summary, written by optimizationLoop and read by setDiagnostics + + // Guarded by optimization_requested_mutex_ + std::mutex optimization_requested_mutex_; //!< Required condition variable mutex + ros::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers a warning if exceeded. + bool optimization_request_; //!< Flag to trigger a new optimization + std::condition_variable optimization_requested_; //!< Condition variable used by the optimization thread to wait + //!< until a new optimization is requested by the main thread + + // Guarded by start_time_mutex_ + mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable + ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction + + // Ordering ROS objects with callbacks last + ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency + ros::ServiceServer reset_service_server_; //!< Service that resets the optimizer to its initial state + + /** + * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called + * + * All new transactions that will be applied to the graph are provided. This does not include the marginal + * transaction that is computed later. + * + * This method will be called before the graph has been updated. + * + * @param[in] new_transaction All new, non-marginal-related transactions that *will be* applied to the graph + */ + virtual void preprocessMarginalization(const fuse_core::Transaction& new_transaction) = 0; + + /** + * @brief Compute the set of variables that should be marginalized from the graph + * + * This will be called after \p preprocessMarginalization() and after the graph has been updated with the any + * previous marginal transactions and new transactions. + * + * @return A container with the set of variables to marginalize out. Order of the variables is not specified. + */ + virtual std::vector computeVariablesToMarginalize() = 0; + + /** + * @brief Perform any required post-marginalization bookkeeping + * + * The transaction containing the actual changed to the graph is supplied. This will be called before the + * transaction is actually applied to the graph. + * + * @param[in] marginal_transaction The actual changes to the graph caused my marginalizing out the requested + * variables. + */ + virtual void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) = 0; + + /** + * @brief Perform any required operations whenever the optimizer is reset + */ + virtual void onReset() = 0; + + /** + * @brief Automatically start the smoother if no ignition sensors are specified + */ + void autostart(); + + /** + * @brief Function that optimizes all constraints, designed to be run in a separate thread. + * + * This function waits for an optimization or shutdown signal, then either calls optimize() or exits appropriately. + */ + void optimizationLoop(); + + /** + * @brief Callback fired at a fixed frequency to trigger a new optimization cycle. + * + * This callback checks if a current optimization cycle is still running. If not, a new optimization cycle is started. + * If so, we simply wait for the next timer event to start another optimization cycle. + * + * @param event The ROS timer event metadata + */ + void optimizerTimerCallback(const ros::TimerEvent& event); + + /** + * @brief Generate motion model constraints for pending transactions and combine them into a single transaction + * + * Transactions are processed sequentially based on timestamp. If motion models are successfully generated for a + * pending transactions, that transaction is merged into the combined transaction and removed from the pending + * queue. If motion models fail to generate after the configured transaction_timeout_, the transaction will be + * deleted from the pending queue and a warning will be displayed. + * + * @param[out] transaction The transaction object to be augmented with pending motion model and sensor transactions + */ + void processQueue(fuse_core::Transaction& transaction); + + /** + * @brief Service callback that resets the optimizer to its original state + */ + bool resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + + /** + * @brief Thread-safe read-only access to the optimizer start time + */ + ros::Time getStartTime() const + { + std::lock_guard lock(start_time_mutex_); + return start_time_; + } + + /** + * @brief Thread-safe write access to the optimizer start time + */ + void setStartTime(const ros::Time& start_time) + { + std::lock_guard lock(start_time_mutex_); + start_time_ = start_time; + } + + /** + * @brief Callback fired every time the SensorModel plugin creates a new transaction + * + * This callback is responsible for ensuring all associated motion models are applied before any other processing + * takes place. See Optimizer::applyMotionModels() for a helper function that does just that. + * + * This implementation shares ownership of the transaction object. + * + * @param[in] name The name of the sensor that produced the Transaction + * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin + */ + void transactionCallback( + const std::string& sensor_name, + fuse_core::Transaction::SharedPtr transaction) override; + + /** + * @brief Update and publish diagnotics + * @param[in] status The diagnostic status + */ + void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override; +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_WINDOWED_OPTIMIZER_H diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h new file mode 100644 index 000000000..95c37623d --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h @@ -0,0 +1,114 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_OPTIMIZERS_WINDOWED_OPTIMIZER_PARAMS_H +#define FUSE_OPTIMIZERS_WINDOWED_OPTIMIZER_PARAMS_H + +#include +#include +#include +#include + +#include + +#include +#include +#include + + +namespace fuse_optimizers +{ + +/** + * @brief Defines the set of parameters required by the fuse_optimizers::WindowedOptimizer base class + */ +struct WindowedOptimizerParams +{ +public: + /** + * @brief The target duration for optimization cycles + * + * If an optimization takes longer than expected, an optimization cycle may be skipped. The optimization period + * may be specified in either the "optimization_period" parameter in seconds, or in the "optimization_frequency" + * parameter in Hz. + */ + ros::Duration optimization_period { 0.1 }; + + /** + * @brief The topic name of the advertised reset service + */ + std::string reset_service { "~reset" }; + + /** + * @brief The maximum time to wait for motion models to be generated for a received transaction. + * + * Transactions are processed sequentially, so no new transactions will be added to the graph while waiting for + * motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue. + */ + ros::Duration transaction_timeout { 0.1 }; + + /** + * @brief Ceres Solver::Options object that controls various aspects of the optimizer. + */ + ceres::Solver::Options solver_options; + + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) + { + // Read settings from the parameter server + if (nh.hasParam("optimization_frequency")) + { + double optimization_frequency{ 1.0 / optimization_period.toSec() }; + fuse_core::getPositiveParam(nh, "optimization_frequency", optimization_frequency); + optimization_period.fromSec(1.0 / optimization_frequency); + } + else + { + fuse_core::getPositiveParam(nh, "optimization_period", optimization_period); + } + + nh.getParam("reset_service", reset_service); + + fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout); + + fuse_core::loadSolverOptionsFromROS(ros::NodeHandle(nh, "solver_options"), solver_options); + } +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_WINDOWED_OPTIMIZER_PARAMS_H diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 1ea2d36b0..1f576cdaa 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -46,614 +46,51 @@ #include #include #include +#include #include - -namespace -{ -/** - * @brief Delete an element from the vector using a reverse iterator - * - * @param[in] container The contain to delete from - * @param[in] position A reverse iterator that access the element to be erased - * @return A reverse iterator pointing to the element after the erased element - */ -template -typename std::vector::reverse_iterator erase( - std::vector& container, - typename std::vector::reverse_iterator position) -{ - // Reverse iterators are weird - // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator - // Basically a reverse iterator access the element one place before the element it points at. - // E.g. The reverse iterator rbegin points at end, but accesses end-1. - // When you delete something, you need to increment the reverse iterator first, then convert it to a standard - // iterator for the erase operation. - std::advance(position, 1); - container.erase(position.base()); - return position; -} -} // namespace - namespace fuse_optimizers { -FixedLagSmoother::FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), - ignited_(false), - optimization_running_(true), - started_(false), - optimization_request_(false) -{ - params_.loadFromROS(private_node_handle); - - // Test for auto-start - autostart(); - - // Start the optimization thread - optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this); - - // Configure a timer to trigger optimizations - optimize_timer_ = node_handle_.createTimer( - params_.optimization_period, - &FixedLagSmoother::optimizerTimerCallback, - this); - - // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = node_handle_.advertiseService( - ros::names::resolve(params_.reset_service), - &FixedLagSmoother::resetServiceCallback, - this); -} - -FixedLagSmoother::~FixedLagSmoother() -{ - // Wake up any sleeping threads - optimization_running_ = false; - optimization_requested_.notify_all(); - // Wait for the threads to shutdown - if (optimization_thread_.joinable()) - { - optimization_thread_.join(); - } -} - -void FixedLagSmoother::autostart() -{ - if (std::none_of(sensor_models_.begin(), sensor_models_.end(), - [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) + FixedLagSmoother::FixedLagSmoother( + fuse_core::Graph::UniquePtr graph, + const ParameterType ¶ms, + const ros::NodeHandle &node_handle, + const ros::NodeHandle &private_node_handle) : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle) { - // No ignition sensors were provided. Auto-start. - started_ = true; - setStartTime(ros::Time(0, 0)); - ROS_INFO_STREAM("No ignition sensors were specified. Optimization will begin immediately."); } -} - -void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction& new_transaction) -{ - timestamp_tracking_.addNewTransaction(new_transaction); -} - -ros::Time FixedLagSmoother::computeLagExpirationTime() const -{ - // Find the most recent variable timestamp - auto start_time = getStartTime(); - auto now = timestamp_tracking_.currentStamp(); - // Then carefully subtract the lag duration. ROS Time objects do not handle negative values. - return (start_time + params_.lag_duration < now) ? now - params_.lag_duration : start_time; -} - -std::vector FixedLagSmoother::computeVariablesToMarginalize(const ros::Time& lag_expiration) -{ - auto marginalize_variable_uuids = std::vector(); - timestamp_tracking_.query(lag_expiration, std::back_inserter(marginalize_variable_uuids)); - return marginalize_variable_uuids; -} - -void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) -{ - timestamp_tracking_.addMarginalTransaction(marginal_transaction); -} -void FixedLagSmoother::optimizationLoop() -{ - auto exit_wait_condition = [this]() - { - return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); - }; - // Optimize constraints until told to exit - while (ros::ok() && optimization_running_) + ros::Time FixedLagSmoother::computeLagExpirationTime() const { - // Wait for the next signal to start the next optimization cycle - auto optimization_deadline = ros::Time(0, 0); - { - std::unique_lock lock(optimization_requested_mutex_); - optimization_requested_.wait(lock, exit_wait_condition); - optimization_request_ = false; - optimization_deadline = optimization_deadline_; - } - // If a shutdown is requested, exit now. - if (!optimization_running_ || !ros::ok()) - { - break; - } - // Optimize - { - std::lock_guard lock(optimization_mutex_); - // Apply motion models - auto new_transaction = fuse_core::Transaction::make_shared(); - // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ - // We do this to ensure state of the graph does not change between unlocking the pending_transactions - // queue and obtaining the lock for the graph. But we have now obtained two different locks. If we are - // not extremely careful, we could get a deadlock. - processQueue(*new_transaction, lag_expiration_); - // Skip this optimization cycle if the transaction is empty because something failed while processing the pending - // transactions queue. - if (new_transaction->empty()) - { - continue; - } - // check if new transaction has added constraints to variables that are to be marginalized - std::vector faulty_constraints; - for(auto& c: new_transaction->addedConstraints()) - { - for(auto var_uuid: c.variables()) - { - for (auto marginal_uuid : marginal_transaction_.removedVariables()) - { - if (var_uuid == marginal_uuid) - { - faulty_constraints.push_back(c.uuid()); - break; - } - } - } - } - if(faulty_constraints.size() > 0) - { - ROS_WARN_STREAM("Removing invalid constraints."); - for(auto& faulty_constraint: faulty_constraints) - { - new_transaction->removeConstraint(faulty_constraint); - } - } - // Prepare for selecting the marginal variables - preprocessMarginalization(*new_transaction); - // Combine the new transactions with any marginal transaction from the end of the last cycle - new_transaction->merge(marginal_transaction_); - // Update the graph - try - { - graph_->update(*new_transaction); - } - catch (const std::exception& ex) - { - std::ostringstream oss; - oss << "Graph:\n"; - graph_->print(oss); - oss << "\nTransaction:\n"; - new_transaction->print(oss); - - ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() - << "\nLeaving optimization loop and requesting " - "node shutdown...\n" << oss.str()); - ros::requestShutdown(); - break; - } - // Optimize the entire graph - summary_ = graph_->optimize(params_.solver_options); - - // Optimization is complete. Notify all the things about the graph changes. - const auto new_transaction_stamp = new_transaction->stamp(); - notify(std::move(new_transaction), graph_->clone()); - - // Abort if optimization failed. Not converging is not a failure because the solution found is usable. - if (!summary_.IsSolutionUsable()) - { - ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); - ROS_INFO_STREAM(summary_.FullReport()); - ros::requestShutdown(); - break; - } - - // Compute a transaction that marginalizes out those variables. - lag_expiration_ = computeLagExpirationTime(); - marginal_transaction_ = fuse_constraints::marginalizeVariables( - ros::this_node::getName(), - computeVariablesToMarginalize(lag_expiration_), - *graph_); - // Perform any post-marginal cleanup - postprocessMarginalization(marginal_transaction_); - // Note: The marginal transaction will not be applied until the next optimization iteration - // Log a warning if the optimization took too long - auto optimization_complete = ros::Time::now(); - if (optimization_complete > optimization_deadline) - { - ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " - << (optimization_complete - optimization_deadline) << "s"); - } - } + // Find the most recent variable timestamp + auto start_time = getStartTime(); + auto now = timestamp_tracking_.currentStamp(); + // Then carefully subtract the lag duration. ROS Time objects do not handle negative values. + return (start_time + params_.lag_duration < now) ? now - params_.lag_duration : start_time; } -} -void FixedLagSmoother::optimizerTimerCallback(const ros::TimerEvent& event) -{ - // If an "ignition" transaction hasn't been received, then we can't do anything yet. - if (!started_) - { - return; - } - // If there is some pending work, trigger the next optimization cycle. - // If the optimizer has not completed the previous optimization cycle, then it - // will not be waiting on the condition variable signal, so nothing will happen. - auto optimization_request = false; - { - std::lock_guard lock(pending_transactions_mutex_); - optimization_request = !pending_transactions_.empty(); - } - if (optimization_request) + void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction &new_transaction) { - { - std::lock_guard lock(optimization_requested_mutex_); - optimization_request_ = true; - optimization_deadline_ = event.current_expected + params_.optimization_period; - } - optimization_requested_.notify_one(); + timestamp_tracking_.addNewTransaction(new_transaction); } -} -void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const ros::Time& lag_expiration) -{ - // We need to get the pending transactions from the queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - - if (pending_transactions_.empty()) + std::vector FixedLagSmoother::computeVariablesToMarginalize() { - return; + auto lag_expiration = computeLagExpirationTime(); + auto marginalize_variable_uuids = std::vector(); + timestamp_tracking_.query(lag_expiration, std::back_inserter(marginalize_variable_uuids)); + return marginalize_variable_uuids; } - // If we just started because an ignition sensor transaction was received, we try to process it individually. This is - // important because we need to update the graph with the ignition sensor transaction in order to get the motion - // models notified of the initial state. The motion models will typically maintain a state history in order to create - // motion model constraints with the optimized variables from the updated graph. If we do not process the ignition - // sensor transaction individually, the motion model constraints created for the other queued transactions will not be - // able to use any optimized variables from the graph because it is not been optimized yet, and they will have to use - // a default zero state instead. This can easily lead to local minima because the variables in the graph are not - // initialized properly, i.e. they do not take the ignition sensor transaction into account. - if (ignited_) + void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction) { - // The ignition sensor transaction is assumed to be at the end of the queue, because it must be the oldest one. - // If there is more than one ignition sensor transaction in the queue, it is always the oldest one that started - // things up. - ignited_ = false; - - const auto transaction_rbegin = pending_transactions_.rbegin(); - auto& element = *transaction_rbegin; - if (!sensor_models_.at(element.sensor_name).ignition) - { - // We just started, but the oldest transaction is not from an ignition sensor. We will still process the - // transaction, but we do not enforce it is processed individually. - ROS_ERROR_STREAM("The queued transaction with timestamp " << element.stamp() << " from sensor " << - element.sensor_name << " is not an ignition sensor transaction. " << - "This transaction will not be processed individually."); - } - else - { - if (applyMotionModels(element.sensor_name, *element.transaction)) - { - // Processing was successful. Add the results to the final transaction, delete this one, and return, so the - // transaction from the ignition sensor is processed individually. - transaction.merge(*element.transaction, true); - erase(pending_transactions_, transaction_rbegin); - } - else - { - // The motion model processing failed. When this happens to an ignition sensor transaction there is no point on - // trying again next time, so we ignore this transaction. - ROS_ERROR_STREAM("The queued ignition transaction with timestamp " << element.stamp() << " from sensor " << - element.sensor_name << " could not be processed. Ignoring this ignition transaction."); - - // Remove the ignition transaction that just failed and purge all transactions after it. But if we find another - // ignition transaction, we schedule it to be processed in the next optimization cycle. - erase(pending_transactions_, transaction_rbegin); - - const auto pending_ignition_transaction_iter = - std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), - [this](const auto& element) { // NOLINT(whitespace/braces) - return sensor_models_.at(element.sensor_name).ignition; - }); // NOLINT(whitespace/braces) - if (pending_ignition_transaction_iter == pending_transactions_.rend()) - { - // There is no other ignition transaction pending. We simply roll back to not started state and all other - // pending transactions will be handled later in the transaction callback, as usual. - started_ = false; - } - else - { - // Erase all transactions before the other ignition transaction pending. This other ignition transaction will - // be processed in the next optimization cycle. - pending_transactions_.erase(pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); - ignited_ = true; - } - } - - // There are no more pending transactions to process in this optimization cycle, or they should be processed in - // the next one. - return; - } + timestamp_tracking_.addMarginalTransaction(marginal_transaction); } - // Use the most recent transaction time as the current time - const auto current_time = pending_transactions_.front().stamp(); - - // Attempt to process each pending transaction - auto sensor_blacklist = std::vector(); - auto transaction_riter = pending_transactions_.rbegin(); - while (transaction_riter != pending_transactions_.rend()) + void FixedLagSmoother::onReset() { - auto& element = *transaction_riter; - const auto& min_stamp = element.minStamp(); - if (min_stamp < lag_expiration) - { - ROS_DEBUG_STREAM("The current lag expiration time is " << lag_expiration << ". The queued transaction with " - "timestamp " << element.stamp() << " from sensor " << element.sensor_name << " has a minimum " - "involved timestamp of " << min_stamp << ", which is " << (lag_expiration - min_stamp) << - " seconds too old. Ignoring this transaction."); - transaction_riter = erase(pending_transactions_, transaction_riter); - } - else if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) - { - // We should not process transactions from this sensor - ++transaction_riter; - } - else if (applyMotionModels(element.sensor_name, *element.transaction)) - { - // Processing was successful. Add the results to the final transaction, delete this one, and move to the next. - transaction.merge(*element.transaction, true); - transaction_riter = erase(pending_transactions_, transaction_riter); - } - else - { - // The motion model processing failed. - // Check the transaction timeout to determine if it should be removed or skipped. - const auto& max_stamp = element.maxStamp(); - if (max_stamp + params_.transaction_timeout < current_time) - { - // Warn that this transaction has expired, then skip it. - ROS_ERROR_STREAM("The queued transaction with timestamp " << element.stamp() << " and maximum " - "involved stamp of " << max_stamp << " from sensor " << element.sensor_name << - " could not be processed after " << (current_time - max_stamp) << " seconds, " - "which is greater than the 'transaction_timeout' value of " << - params_.transaction_timeout << ". Ignoring this transaction."); - transaction_riter = erase(pending_transactions_, transaction_riter); - } - else - { - // The motion model failed. Stop further processing of this sensor and try again next time. - sensor_blacklist.push_back(element.sensor_name); - ++transaction_riter; - } - } - } -} - -bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - // Tell all the plugins to stop - stopPlugins(); - // Reset the optimizer state - { - std::lock_guard lock(optimization_requested_mutex_); - optimization_request_ = false; - } - started_ = false; - ignited_ = false; - setStartTime(ros::Time(0, 0)); - // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the - // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to - // prevent the possibility of deadlocks. - { - std::lock_guard lock(optimization_mutex_); - // Clear all pending transactions - { - std::lock_guard lock(pending_transactions_mutex_); - pending_transactions_.clear(); - } - // Clear the graph and marginal tracking states - graph_->clear(); - marginal_transaction_ = fuse_core::Transaction(); timestamp_tracking_.clear(); lag_expiration_ = ros::Time(0, 0); } - // Tell all the plugins to start - startPlugins(); - // Test for auto-start - autostart(); - - return true; -} - -void FixedLagSmoother::transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) -{ - // If this transaction occurs before the start time, just ignore it - auto start_time = getStartTime(); - const auto max_time = transaction->maxStamp(); - if (started_ && max_time < start_time) - { - ROS_DEBUG_STREAM("Received a transaction before the start time from sensor '" << sensor_name << "'.\n" << - " start_time: " << start_time << ", maximum involved stamp: " << max_time << - ", difference: " << (start_time - max_time) << "s"); - return; - } - { - // We need to add the new transaction to the pending_transactions_ queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - - // Add the new transaction to the pending set - // The pending set is arranged "smallest stamp last" to making popping off the back more efficient - auto comparator = [](const ros::Time& value, const TransactionQueueElement& element) - { - return value >= element.stamp(); - }; - auto position = std::upper_bound( - pending_transactions_.begin(), - pending_transactions_.end(), - transaction->stamp(), - comparator); - position = pending_transactions_.insert(position, {sensor_name, std::move(transaction)}); // NOLINT - - // If we haven't "started" yet.. - if (!started_) - { - // ...check if we should - if (sensor_models_.at(sensor_name).ignition) - { - started_ = true; - ignited_ = true; - start_time = position->minStamp(); - setStartTime(start_time); - - // And purge out old transactions - // - Either before or exactly at the start time - // - Or with a minimum time before the minimum time of this ignition sensor transaction - // - // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we can use - // std::as_const: https://en.cppreference.com/w/cpp/utility/as_const - pending_transactions_.erase( - std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); - } - else - { - // And purge out old transactions to limit the pending size while waiting for an ignition sensor - auto purge_time = ros::Time(0, 0); - auto last_pending_time = pending_transactions_.front().stamp(); - if (ros::Time(0, 0) + params_.transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives - { - purge_time = last_pending_time - params_.transaction_timeout; - } - - while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) - { - pending_transactions_.pop_back(); - } - } - } - } -} - -/** - * @brief Make a diagnostic_msgs::DiagnosticStatus message filling in the level and message - * - * @param[in] level The diagnostic status level - * @param[in] message The diagnostic status message - */ -diagnostic_msgs::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string& message) -{ - diagnostic_msgs::DiagnosticStatus status; - - status.level = level; - status.message = message; - - return status; -} - -/** - * @brief Helper function to generate the diagnostic status for each optimization termination type - * - * The termination type -> diagnostic status mapping is as follows: - * - * - CONVERGENCE, USER_SUCCESS -> OK - * - NO_CONVERGENCE -> WARN - * - FAILURE, USER_FAILURE -> ERROR (default) - * - * @param[in] termination_type The optimization termination type - * @return The diagnostic status with the level and message corresponding to the optimization termination type - */ -diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type) -{ - switch (termination_type) - { - case ceres::TerminationType::CONVERGENCE: - case ceres::TerminationType::USER_SUCCESS: - return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged"); - case ceres::TerminationType::NO_CONVERGENCE: - return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::WARN, "Optimization didn't converge"); - default: - return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::ERROR, "Optimization failed"); - } -} - -void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) -{ - Optimizer::setDiagnostics(status); - - // Load std::atomic flag that indicates whether the optimizer has started or not - const bool started = started_; - - status.add("Started", started); - { - std::lock_guard lock(pending_transactions_mutex_); - status.add("Pending Transactions", pending_transactions_.size()); - } - - if (started) - { - // Add some optimization summary report fields to the diagnostics status if the optimizer has started - auto summary = decltype(summary_)(); - { - const std::unique_lock lock(optimization_mutex_, std::try_to_lock); - if (lock) - { - summary = summary_; - } - else - { - status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimization running"); - } - } - - if (summary.total_time_in_seconds >= 0.0) // This is -1 for the default-constructed summary object - { - status.add("Optimization Termination Type", ceres::TerminationTypeToString(summary.termination_type)); - status.add("Optimization Total Time [s]", summary.total_time_in_seconds); - status.add("Optimization Iterations", summary.iterations.size()); - status.add("Initial Cost", summary.initial_cost); - status.add("Final Cost", summary.final_cost); - - status.mergeSummary(terminationTypeToDiagnosticStatus(summary.termination_type)); - } - - // Add time since the last optimization request time. This is useful to detect if no transactions are received for - // too long - auto optimization_deadline = decltype(optimization_deadline_)(); - { - const std::unique_lock lock(optimization_requested_mutex_, std::try_to_lock); - if (lock) - { - optimization_deadline = optimization_deadline_; - } - } - - if (!optimization_deadline.isZero()) // This is zero for the default-constructed optimization_deadline object - { - const auto optimization_request_time = optimization_deadline - params_.optimization_period; - const auto time_since_last_optimization_request = ros::Time::now() - optimization_request_time; - status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.toSec()); - } - } -} -} // namespace fuse_optimizers +} // namespace fuse_optimizers diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 25c8230fb..6323c5e49 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include @@ -43,7 +44,9 @@ int main(int argc, char **argv) ros::NodeHandle private_node_handle("~"); fuse_graphs::HashGraphParams hash_graph_params; hash_graph_params.loadFromROS(private_node_handle); - fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + fuse_optimizers::FixedLagSmootherParams smoother_params; + smoother_params.loadFromROS(private_node_handle); + fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params), smoother_params); ros::spin(); return 0; diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp new file mode 100644 index 000000000..974e88efa --- /dev/null +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -0,0 +1,613 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +namespace +{ +/** + * @brief Delete an element from the vector using a reverse iterator + * + * @param[in] container The contain to delete from + * @param[in] position A reverse iterator that access the element to be erased + * @return A reverse iterator pointing to the element after the erased element + */ +template +typename std::vector::reverse_iterator erase( + std::vector& container, + typename std::vector::reverse_iterator position) +{ + // Reverse iterators are weird + // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator + // Basically a reverse iterator access the element one place before the element it points at. + // E.g. The reverse iterator rbegin points at end, but accesses end-1. + // When you delete something, you need to increment the reverse iterator first, then convert it to a standard + // iterator for the erase operation. + std::advance(position, 1); + container.erase(position.base()); + return position; +} +} // namespace + +namespace fuse_optimizers +{ + +WindowedOptimizer::WindowedOptimizer( + fuse_core::Graph::UniquePtr graph, + const ParameterType& params, + const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) : + fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), + params_(params), + ignited_(false), + optimization_running_(true), + started_(false), + optimization_request_(false) +{ + // Test for auto-start + autostart(); + + // Start the optimization thread + optimization_thread_ = std::thread(&WindowedOptimizer::optimizationLoop, this); + + // Configure a timer to trigger optimizations + optimize_timer_ = node_handle_.createTimer( + params_.optimization_period, + &WindowedOptimizer::optimizerTimerCallback, + this); + + // Advertise a service that resets the optimizer to its initial state + reset_service_server_ = node_handle_.advertiseService( + ros::names::resolve(params_.reset_service), + &WindowedOptimizer::resetServiceCallback, + this); +} + +WindowedOptimizer::~WindowedOptimizer() +{ + // Wake up any sleeping threads + optimization_running_ = false; + optimization_requested_.notify_all(); + // Wait for the threads to shutdown + if (optimization_thread_.joinable()) + { + optimization_thread_.join(); + } +} + +void WindowedOptimizer::autostart() +{ + if (std::none_of(sensor_models_.begin(), sensor_models_.end(), + [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) + { + // No ignition sensors were provided. Auto-start. + started_ = true; + setStartTime(ros::Time(0, 0)); + ROS_INFO_STREAM("No ignition sensors were specified. Optimization will begin immediately."); + } +} + +void WindowedOptimizer::optimizationLoop() +{ + auto exit_wait_condition = [this]() + { + return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); + }; + // Optimize constraints until told to exit + while (ros::ok() && optimization_running_) + { + // Wait for the next signal to start the next optimization cycle + auto optimization_deadline = ros::Time(0, 0); + { + std::unique_lock lock(optimization_requested_mutex_); + optimization_requested_.wait(lock, exit_wait_condition); + optimization_request_ = false; + optimization_deadline = optimization_deadline_; + } + // If a shutdown is requested, exit now. + if (!optimization_running_ || !ros::ok()) + { + break; + } + // Optimize + { + std::lock_guard lock(optimization_mutex_); + // Apply motion models + auto new_transaction = fuse_core::Transaction::make_shared(); + // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ + // We do this to ensure state of the graph does not change between unlocking the pending_transactions + // queue and obtaining the lock for the graph. But we have now obtained two different locks. If we are + // not extremely careful, we could get a deadlock. + processQueue(*new_transaction); + // Skip this optimization cycle if the transaction is empty because something failed while processing the pending + // transactions queue. + if (new_transaction->empty()) + { + continue; + } + // Prepare for selecting the marginal variables -- Delegated to derived classes + preprocessMarginalization(*new_transaction); + // Combine the new transactions with any marginal transaction from the end of the last cycle + new_transaction->merge(marginal_transaction_); + // Update the graph + try + { + graph_->update(*new_transaction); + } + catch (const std::exception& ex) + { + std::ostringstream oss; + oss << "Graph:\n"; + graph_->print(oss); + oss << "\nTransaction:\n"; + new_transaction->print(oss); + + ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() + << "\nLeaving optimization loop and requesting " + "node shutdown...\n" << oss.str()); + ros::requestShutdown(); + break; + } + // Optimize the entire graph + summary_ = graph_->optimize(params_.solver_options); + + // Optimization is complete. Notify all the things about the graph changes. + const auto new_transaction_stamp = new_transaction->stamp(); + notify(std::move(new_transaction), graph_->clone()); + + // Abort if optimization failed. Not converging is not a failure because the solution found is usable. + if (!summary_.IsSolutionUsable()) + { + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + ROS_INFO_STREAM(summary_.FullReport()); + ros::requestShutdown(); + break; + } + + // Marginal out any variables outside of the current "window" + // Determination of which variables to marginalize is delegated to derived classes + auto variables_to_marginalize = computeVariablesToMarginalize(); + marginal_transaction_ = fuse_constraints::marginalizeVariables( + ros::this_node::getName(), + variables_to_marginalize, + *graph_); + // Perform any post-marginal cleanup -- Delegated to derived classes + postprocessMarginalization(marginal_transaction_); + // Note: The marginal transaction will not be applied until the next optimization iteration + // Log a warning if the optimization took too long + auto optimization_complete = ros::Time::now(); + if (optimization_complete > optimization_deadline) + { + ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline) << "s"); + } + } + } +} + +void WindowedOptimizer::optimizerTimerCallback(const ros::TimerEvent& event) +{ + // If an "ignition" transaction hasn't been received, then we can't do anything yet. + if (!started_) + { + return; + } + // If there is some pending work, trigger the next optimization cycle. + // If the optimizer has not completed the previous optimization cycle, then it + // will not be waiting on the condition variable signal, so nothing will happen. + auto optimization_request = false; + { + std::lock_guard lock(pending_transactions_mutex_); + optimization_request = !pending_transactions_.empty(); + } + if (optimization_request) + { + { + std::lock_guard lock(optimization_requested_mutex_); + optimization_request_ = true; + optimization_deadline_ = event.current_expected + params_.optimization_period; + } + optimization_requested_.notify_one(); + } +} + +void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) +{ + // We need to get the pending transactions from the queue + std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + + if (pending_transactions_.empty()) + { + return; + } + + // If we just started because an ignition sensor transaction was received, we try to process it individually. This is + // important because we need to update the graph with the ignition sensor transaction in order to get the motion + // models notified of the initial state. The motion models will typically maintain a state history in order to create + // motion model constraints with the optimized variables from the updated graph. If we do not process the ignition + // sensor transaction individually, the motion model constraints created for the other queued transactions will not be + // able to use any optimized variables from the graph because it is not been optimized yet, and they will have to use + // a default zero state instead. This can easily lead to local minima because the variables in the graph are not + // initialized properly, i.e. they do not take the ignition sensor transaction into account. + if (ignited_) + { + // The ignition sensor transaction is assumed to be at the end of the queue, because it must be the oldest one. + // If there is more than one ignition sensor transaction in the queue, it is always the oldest one that started + // things up. + ignited_ = false; + + const auto transaction_rbegin = pending_transactions_.rbegin(); + auto& element = *transaction_rbegin; + if (!sensor_models_.at(element.sensor_name).ignition) + { + // We just started, but the oldest transaction is not from an ignition sensor. We will still process the + // transaction, but we do not enforce it is processed individually. + ROS_ERROR_STREAM("The queued transaction with timestamp " << element.stamp() << " from sensor " << + element.sensor_name << " is not an ignition sensor transaction. " << + "This transaction will not be processed individually."); + } + else + { + if (applyMotionModels(element.sensor_name, *element.transaction)) + { + // Processing was successful. Add the results to the final transaction, delete this one, and return, so the + // transaction from the ignition sensor is processed individually. + transaction.merge(*element.transaction, true); + erase(pending_transactions_, transaction_rbegin); + } + else + { + // The motion model processing failed. When this happens to an ignition sensor transaction there is no point on + // trying again next time, so we ignore this transaction. + ROS_ERROR_STREAM("The queued ignition transaction with timestamp " << element.stamp() << " from sensor " << + element.sensor_name << " could not be processed. Ignoring this ignition transaction."); + + // Remove the ignition transaction that just failed and purge all transactions after it. But if we find another + // ignition transaction, we schedule it to be processed in the next optimization cycle. + erase(pending_transactions_, transaction_rbegin); + + const auto pending_ignition_transaction_iter = + std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), + [this](const auto& element) { // NOLINT(whitespace/braces) + return sensor_models_.at(element.sensor_name).ignition; + }); // NOLINT(whitespace/braces) + if (pending_ignition_transaction_iter == pending_transactions_.rend()) + { + // There is no other ignition transaction pending. We simply roll back to not started state and all other + // pending transactions will be handled later in the transaction callback, as usual. + started_ = false; + } + else + { + // Erase all transactions before the other ignition transaction pending. This other ignition transaction will + // be processed in the next optimization cycle. + pending_transactions_.erase(pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); + ignited_ = true; + } + } + + // There are no more pending transactions to process in this optimization cycle, or they should be processed in + // the next one. + return; + } + } + + // Use the most recent transaction time as the current time + const auto current_time = pending_transactions_.front().stamp(); + + // Attempt to process each pending transaction + auto sensor_blacklist = std::vector(); + auto transaction_riter = pending_transactions_.rbegin(); + while (transaction_riter != pending_transactions_.rend()) + { + auto& element = *transaction_riter; +/// @todo(swilliams) This is fairly specific to the fixed-lag smoother. I need to think of a way to generalize this +/// operation and push the implementation into the derived class. Maybe a filterQueuedTransactions() virtual method? +// const auto& min_stamp = element.minStamp(); +// if (min_stamp < lag_expiration) +// { +// ROS_DEBUG_STREAM("The current lag expiration time is " << lag_expiration << ". The queued transaction with " +// "timestamp " << element.stamp() << " from sensor " << element.sensor_name << " has a minimum " +// "involved timestamp of " << min_stamp << ", which is " << (lag_expiration - min_stamp) << +// " seconds too old. Ignoring this transaction."); +// transaction_riter = erase(pending_transactions_, transaction_riter); +// } +// else if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) + if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) + { + // We should not process transactions from this sensor + ++transaction_riter; + } + else if (applyMotionModels(element.sensor_name, *element.transaction)) + { + // Processing was successful. Add the results to the final transaction, delete this one, and move to the next. + transaction.merge(*element.transaction, true); + transaction_riter = erase(pending_transactions_, transaction_riter); + } + else + { + // The motion model processing failed. + // Check the transaction timeout to determine if it should be removed or skipped. + const auto& max_stamp = element.maxStamp(); + if (max_stamp + params_.transaction_timeout < current_time) + { + // Warn that this transaction has expired, then skip it. + ROS_ERROR_STREAM("The queued transaction with timestamp " << element.stamp() << " and maximum " + "involved stamp of " << max_stamp << " from sensor " << element.sensor_name << + " could not be processed after " << (current_time - max_stamp) << " seconds, " + "which is greater than the 'transaction_timeout' value of " << + params_.transaction_timeout << ". Ignoring this transaction."); + transaction_riter = erase(pending_transactions_, transaction_riter); + } + else + { + // The motion model failed. Stop further processing of this sensor and try again next time. + sensor_blacklist.push_back(element.sensor_name); + ++transaction_riter; + } + } + } +} + +bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + // Tell all the plugins to stop + stopPlugins(); + // Reset the optimizer state + { + std::lock_guard lock(optimization_requested_mutex_); + optimization_request_ = false; + } + started_ = false; + ignited_ = false; + setStartTime(ros::Time(0, 0)); + // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the + // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to + // prevent the possibility of deadlocks. + { + std::lock_guard lock(optimization_mutex_); + // Clear all pending transactions + { + std::lock_guard lock(pending_transactions_mutex_); + pending_transactions_.clear(); + } + // Clear the graph and marginal tracking states + graph_->clear(); + marginal_transaction_ = fuse_core::Transaction(); + } + // Perform any required reset operations for derived classes + onReset(); + // Tell all the plugins to start + startPlugins(); + // Test for auto-start + autostart(); + + return true; +} + +void WindowedOptimizer::transactionCallback( + const std::string& sensor_name, + fuse_core::Transaction::SharedPtr transaction) +{ + // If this transaction occurs before the start time, just ignore it + auto start_time = getStartTime(); + const auto max_time = transaction->maxStamp(); + if (started_ && max_time < start_time) + { + ROS_DEBUG_STREAM("Received a transaction before the start time from sensor '" << sensor_name << "'.\n" << + " start_time: " << start_time << ", maximum involved stamp: " << max_time << + ", difference: " << (start_time - max_time) << "s"); + return; + } + { + // We need to add the new transaction to the pending_transactions_ queue + std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + + // Add the new transaction to the pending set + // The pending set is arranged "smallest stamp last" to making popping off the back more efficient + auto comparator = [](const ros::Time& value, const TransactionQueueElement& element) + { + return value >= element.stamp(); + }; + auto position = std::upper_bound( + pending_transactions_.begin(), + pending_transactions_.end(), + transaction->stamp(), + comparator); + position = pending_transactions_.insert(position, {sensor_name, std::move(transaction)}); // NOLINT + + // If we haven't "started" yet.. + if (!started_) + { + // ...check if we should + if (sensor_models_.at(sensor_name).ignition) + { + started_ = true; + ignited_ = true; + start_time = position->minStamp(); + setStartTime(start_time); + + // And purge out old transactions + // - Either before or exactly at the start time + // - Or with a minimum time before the minimum time of this ignition sensor transaction + // + // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we can use + // std::as_const: https://en.cppreference.com/w/cpp/utility/as_const + pending_transactions_.erase( + std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), + [&sensor_name, max_time, + &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); + } + else + { + // And purge out old transactions to limit the pending size while waiting for an ignition sensor + auto purge_time = ros::Time(0, 0); + auto last_pending_time = pending_transactions_.front().stamp(); + if (ros::Time(0, 0) + params_.transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives + { + purge_time = last_pending_time - params_.transaction_timeout; + } + + while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) + { + pending_transactions_.pop_back(); + } + } + } + } +} + +/** + * @brief Make a diagnostic_msgs::DiagnosticStatus message filling in the level and message + * + * @param[in] level The diagnostic status level + * @param[in] message The diagnostic status message + */ +diagnostic_msgs::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string& message) +{ + diagnostic_msgs::DiagnosticStatus status; + + status.level = level; + status.message = message; + + return status; +} + +/** + * @brief Helper function to generate the diagnostic status for each optimization termination type + * + * The termination type -> diagnostic status mapping is as follows: + * + * - CONVERGENCE, USER_SUCCESS -> OK + * - NO_CONVERGENCE -> WARN + * - FAILURE, USER_FAILURE -> ERROR (default) + * + * @param[in] termination_type The optimization termination type + * @return The diagnostic status with the level and message corresponding to the optimization termination type + */ +diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type) +{ + switch (termination_type) + { + case ceres::TerminationType::CONVERGENCE: + case ceres::TerminationType::USER_SUCCESS: + return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged"); + case ceres::TerminationType::NO_CONVERGENCE: + return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::WARN, "Optimization didn't converge"); + default: + return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::ERROR, "Optimization failed"); + } +} + +void WindowedOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) +{ + Optimizer::setDiagnostics(status); + + // Load std::atomic flag that indicates whether the optimizer has started or not + const bool started = started_; + + status.add("Started", started); + { + std::lock_guard lock(pending_transactions_mutex_); + status.add("Pending Transactions", pending_transactions_.size()); + } + + if (started) + { + // Add some optimization summary report fields to the diagnostics status if the optimizer has started + auto summary = decltype(summary_)(); + { + const std::unique_lock lock(optimization_mutex_, std::try_to_lock); + if (lock) + { + summary = summary_; + } + else + { + status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimization running"); + } + } + + if (summary.total_time_in_seconds >= 0.0) // This is -1 for the default-constructed summary object + { + status.add("Optimization Termination Type", ceres::TerminationTypeToString(summary.termination_type)); + status.add("Optimization Total Time [s]", summary.total_time_in_seconds); + status.add("Optimization Iterations", summary.iterations.size()); + status.add("Initial Cost", summary.initial_cost); + status.add("Final Cost", summary.final_cost); + + status.mergeSummary(terminationTypeToDiagnosticStatus(summary.termination_type)); + } + + // Add time since the last optimization request time. This is useful to detect if no transactions are received for + // too long + auto optimization_deadline = decltype(optimization_deadline_)(); + { + const std::unique_lock lock(optimization_requested_mutex_, std::try_to_lock); + if (lock) + { + optimization_deadline = optimization_deadline_; + } + } + + if (!optimization_deadline.isZero()) // This is zero for the default-constructed optimization_deadline object + { + const auto optimization_request_time = optimization_deadline - params_.optimization_period; + const auto time_since_last_optimization_request = ros::Time::now() - optimization_request_time; + status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.toSec()); + } + } +} + +} // namespace fuse_optimizers From c3ff048172579f1b348eb9cdf70202f33f68eca7 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 28 Nov 2021 17:08:31 -0800 Subject: [PATCH 04/17] Cleaned up headers, fixed parameter inheritance issues, documented involved threads, added mutex guards to the fixed-lag smoother. Testing is still needed. --- fuse_core/include/fuse_core/parameter.h | 48 ++++++ .../fuse_optimizers/fixed_lag_smoother.h | 53 +++--- .../fixed_lag_smoother_params.h | 4 +- .../fuse_optimizers/windowed_optimizer.h | 42 +++-- .../windowed_optimizer_params.h | 23 +-- fuse_optimizers/src/fixed_lag_smoother.cpp | 58 +++++-- .../src/fixed_lag_smoother_node.cpp | 4 +- fuse_optimizers/src/windowed_optimizer.cpp | 159 +++++++++--------- 8 files changed, 244 insertions(+), 147 deletions(-) diff --git a/fuse_core/include/fuse_core/parameter.h b/fuse_core/include/fuse_core/parameter.h index dd8d6f9b9..8e4d0ac98 100644 --- a/fuse_core/include/fuse_core/parameter.h +++ b/fuse_core/include/fuse_core/parameter.h @@ -171,6 +171,54 @@ inline fuse_core::Loss::SharedPtr loadLossConfig(const ros::NodeHandle& nh, cons return loss; } +/** + * @brief Checks if a class has a loadFromROS method defined + */ +template +class has_load_from_ros +{ +private: + template struct helper; + + template + static std::true_type has_load_method(helper*); + + template + static std::false_type has_load_method(...); + +public: + static constexpr bool value = std::is_same(nullptr))>::value; +}; + +/** + * @brief Template overload for parameter loading that takes a node handle. Calls a class loadFromROS method. + * @param[in] nh - The namespace from which to load the parameter + * @param[out] params - The parameter to fill with data from the ROS node handle + */ +template< typename ParameterType > +void loadFromROS(const ros::NodeHandle &nh, ParameterType ¶ms) +{ + static_assert( + has_load_from_ros::value, + "loadFromROS() free function called on a type that does not implement the " + "loadFromROS(ros::NodeHandle ros_namespace, ParameterType params) method, nor implements a specialization of the " + "loadFromROS(ros_namespace) free function."); + params.loadFromROS(nh); +} + +/** + * @brief Template overload for parameter loading that takes a node handle. Calls a class loadFromROS method. + * @param[in] nh - The ROS node handle with which to load the params + * @return The parameters, after being loaded from ROS + */ +template< typename ParameterType > +ParameterType loadFromROS(const ros::NodeHandle &nh) +{ + ParameterType params; + params.loadFromROS(nh); + return params; +} + } // namespace fuse_core #endif // FUSE_CORE_PARAMETER_H diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index 749d49a21..6f3e083f6 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -36,24 +36,19 @@ #include #include +#include #include -#include #include -#include -#include +#include +#include +#include -#include -#include -#include #include #include -#include #include - namespace fuse_optimizers { - /** * @brief A fixed-lag smoother implementation that marginalizes out variables that are older than a defined lag time * @@ -108,8 +103,6 @@ class FixedLagSmoother : public WindowedOptimizer SMART_PTR_DEFINITIONS(FixedLagSmoother); using ParameterType = FixedLagSmootherParams; - /// @todo(swilliams) I changed the constructor. This technically breaks API backwards compatibility. Think of a - /// better way to handle supplying the parameters. /** * @brief Constructor * @@ -122,24 +115,34 @@ class FixedLagSmoother : public WindowedOptimizer */ FixedLagSmoother( fuse_core::Graph::UniquePtr graph, - const ParameterType& params, + const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + + /** + * @brief Constructor + * + * The parameters will be loaded from the ROS parameter server using the private node handle + * + * @param[in] graph The derived graph object. This allows different graph implementations to be used + * with the same optimizer code. + * @param[in] node_handle A node handle in the global namespace + * @param[in] private_node_handle A node handle in the node's private namespace + */ + explicit FixedLagSmoother( + fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); protected: // Read-only after construction - ParameterType params_; //!< Configuration settings for this fixed-lag smoother + ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-lag smoother - // Guarded by optimization_mutex_ - /// @todo(swilliams) This should probably be guarded by its own mutex + // Guarded by mutex_ + std::mutex mutex_; //!< Mutex held while the fixed-lag smoother variables are modified ros::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable - /** - * @brief Compute the oldest timestamp that is part of the configured lag window - */ - ros::Time computeLagExpirationTime() const; - /** * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called * @@ -174,6 +177,16 @@ class FixedLagSmoother : public WindowedOptimizer */ void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) override; + /** + * @brief Determine if a new transaction should be applied to the graph + * + * Test if the transaction is within the defined lag window of the smoother. + * + * @param[in] sensor_name - The name of the sensor that produced the provided transaction + * @param[in] transaction - The transaction to be validated + */ + bool validateTransaction(const std::string& sensor_name, const fuse_core::Transaction& transaction) override; + /** * @brief Perform any required operations whenever the optimizer is reset */ diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h index a7c510e8d..da866f292 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h @@ -46,16 +46,16 @@ #include #include - namespace fuse_optimizers { - /** * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class */ struct FixedLagSmootherParams : public WindowedOptimizerParams { public: + SMART_PTR_DEFINITIONS(FixedLagSmootherParams); + /** * @brief The duration of the smoothing window in seconds */ diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h index 00a5c7f3e..5211527d3 100644 --- a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h @@ -36,8 +36,8 @@ #include #include -#include #include +#include #include #include @@ -49,10 +49,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief The WindowedOptimizer acts as a base class for a variety of optimizers that maintain a small window of active * variables. @@ -104,12 +102,6 @@ class WindowedOptimizer : public Optimizer SMART_PTR_DEFINITIONS(WindowedOptimizer); using ParameterType = WindowedOptimizerParams; - /// @todo(swilliams) What is going to happen is the derived class will make a DerivedParameters from the - /// WindowedOptimizerParams base class. It will then pass the derived params object to the WindowedOptimizer, which - /// will keep a copy of just the WindowedOptimizerParams portion of the parameters. This means that the derived - /// class will have two copies of the parameters; changing values in the derived class's copy will have no effect on - /// the base class copy. This is not exactly expected behavior. We could keep a pointer to the derived class params - /// instead, though that looks a bit ugly. I'm open to other suggestions. /** * @brief Constructor * @@ -121,7 +113,7 @@ class WindowedOptimizer : public Optimizer */ WindowedOptimizer( fuse_core::Graph::UniquePtr graph, - const ParameterType& params = ParameterType(), + const ParameterType::SharedPtr& params, const ros::NodeHandle& node_handle = ros::NodeHandle(), const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); @@ -158,7 +150,7 @@ class WindowedOptimizer : public Optimizer // Read-only after construction std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process - ParameterType params_; //!< Configuration settings for this fixed-lag smoother + ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-lag smoother // Inherently thread-safe std::atomic ignited_; //!< Flag indicating the optimizer has received a transaction from an ignition sensor @@ -201,6 +193,8 @@ class WindowedOptimizer : public Optimizer * * This method will be called before the graph has been updated. * + * This method is called inside the optimization thread. + * * @param[in] new_transaction All new, non-marginal-related transactions that *will be* applied to the graph */ virtual void preprocessMarginalization(const fuse_core::Transaction& new_transaction) = 0; @@ -211,6 +205,8 @@ class WindowedOptimizer : public Optimizer * This will be called after \p preprocessMarginalization() and after the graph has been updated with the any * previous marginal transactions and new transactions. * + * This method is called inside the optimization thread. + * * @return A container with the set of variables to marginalize out. Order of the variables is not specified. */ virtual std::vector computeVariablesToMarginalize() = 0; @@ -221,13 +217,33 @@ class WindowedOptimizer : public Optimizer * The transaction containing the actual changed to the graph is supplied. This will be called before the * transaction is actually applied to the graph. * + * This method is called inside the optimization thread. + * * @param[in] marginal_transaction The actual changes to the graph caused my marginalizing out the requested * variables. */ virtual void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) = 0; + /** + * @brief Determine if a new transaction should be applied to the graph + * + * Returns true if the transaction should be included, or false if it should be ignored + * + * This test is performed on a queued transaction before applying motions and adding it to the optimizer. This can + * be used to check if the new transaction is within the optimization window. + * + * This method is called inside the optimization thread. + * + * @param[in] sensor_name - The name of the sensor that produced the provided transaction + * @param[in] transaction - The transaction to be validated + */ + virtual bool validateTransaction(const std::string& sensor_name, const fuse_core::Transaction& transaction) = 0; + /** * @brief Perform any required operations whenever the optimizer is reset + * + * This function is called in the main ROS callback thread, not in the optimization thread like the other virtual + * methods. */ virtual void onReset() = 0; @@ -299,9 +315,7 @@ class WindowedOptimizer : public Optimizer * @param[in] name The name of the sensor that produced the Transaction * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin */ - void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** * @brief Update and publish diagnotics diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h index 95c37623d..5ad4305c4 100644 --- a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h @@ -41,20 +41,18 @@ #include -#include #include -#include - namespace fuse_optimizers { - /** * @brief Defines the set of parameters required by the fuse_optimizers::WindowedOptimizer base class */ struct WindowedOptimizerParams { public: + SMART_PTR_DEFINITIONS(WindowedOptimizerParams); + /** * @brief The target duration for optimization cycles * @@ -69,6 +67,11 @@ struct WindowedOptimizerParams */ std::string reset_service { "~reset" }; + /** + * @brief Ceres Solver::Options object that controls various aspects of the optimizer. + */ + ceres::Solver::Options solver_options; + /** * @brief The maximum time to wait for motion models to be generated for a received transaction. * @@ -77,11 +80,6 @@ struct WindowedOptimizerParams */ ros::Duration transaction_timeout { 0.1 }; - /** - * @brief Ceres Solver::Options object that controls various aspects of the optimizer. - */ - ceres::Solver::Options solver_options; - /** * @brief Method for loading parameter values from ROS. * @@ -92,7 +90,7 @@ struct WindowedOptimizerParams // Read settings from the parameter server if (nh.hasParam("optimization_frequency")) { - double optimization_frequency{ 1.0 / optimization_period.toSec() }; + double optimization_frequency { 1.0 / optimization_period.toSec() }; fuse_core::getPositiveParam(nh, "optimization_frequency", optimization_frequency); optimization_period.fromSec(1.0 / optimization_frequency); } @@ -100,12 +98,9 @@ struct WindowedOptimizerParams { fuse_core::getPositiveParam(nh, "optimization_period", optimization_period); } - nh.getParam("reset_service", reset_service); - - fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout); - fuse_core::loadSolverOptionsFromROS(ros::NodeHandle(nh, "solver_options"), solver_options); + fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout); } }; diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 1f576cdaa..612f78993 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -33,19 +33,18 @@ */ #include -#include #include +#include #include #include -#include -#include +#include +#include +#include +#include -#include #include #include -#include #include -#include #include #include @@ -54,41 +53,68 @@ namespace fuse_optimizers FixedLagSmoother::FixedLagSmoother( fuse_core::Graph::UniquePtr graph, - const ParameterType ¶ms, + const ParameterType::SharedPtr ¶ms, const ros::NodeHandle &node_handle, - const ros::NodeHandle &private_node_handle) : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle) + const ros::NodeHandle &private_node_handle) : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), + params_(params) { } - ros::Time FixedLagSmoother::computeLagExpirationTime() const + FixedLagSmoother::FixedLagSmoother( + fuse_core::Graph::UniquePtr graph, + const ros::NodeHandle &node_handle, + const ros::NodeHandle &private_node_handle) : FixedLagSmoother::FixedLagSmoother(std::move(graph), + ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), + node_handle, + private_node_handle) { - // Find the most recent variable timestamp - auto start_time = getStartTime(); - auto now = timestamp_tracking_.currentStamp(); - // Then carefully subtract the lag duration. ROS Time objects do not handle negative values. - return (start_time + params_.lag_duration < now) ? now - params_.lag_duration : start_time; } void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction &new_transaction) { + std::lock_guard lock(mutex_); timestamp_tracking_.addNewTransaction(new_transaction); } std::vector FixedLagSmoother::computeVariablesToMarginalize() { - auto lag_expiration = computeLagExpirationTime(); + // Find the most recent variable timestamp, then carefully subtract the lag duration. + // ROS Time objects do not handle negative values. + auto start_time = getStartTime(); + + std::lock_guard lock(mutex_); + auto now = timestamp_tracking_.currentStamp(); + lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; auto marginalize_variable_uuids = std::vector(); - timestamp_tracking_.query(lag_expiration, std::back_inserter(marginalize_variable_uuids)); + timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); return marginalize_variable_uuids; } void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction) { + std::lock_guard lock(mutex_); timestamp_tracking_.addMarginalTransaction(marginal_transaction); } + bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction) + { + auto min_stamp = transaction.minStamp(); + std::lock_guard lock(mutex_); + if (min_stamp < lag_expiration_) + { + ROS_DEBUG_STREAM( + "The current lag expiration time is " + << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " + << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is " + << (lag_expiration_ - min_stamp) << " seconds too old. Ignoring this transaction."); + return false; + } + return true; + } + void FixedLagSmoother::onReset() { + std::lock_guard lock(mutex_); timestamp_tracking_.clear(); lag_expiration_ = ros::Time(0, 0); } diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 6323c5e49..942423d2e 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -44,9 +44,7 @@ int main(int argc, char **argv) ros::NodeHandle private_node_handle("~"); fuse_graphs::HashGraphParams hash_graph_params; hash_graph_params.loadFromROS(private_node_handle); - fuse_optimizers::FixedLagSmootherParams smoother_params; - smoother_params.loadFromROS(private_node_handle); - fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params), smoother_params); + fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); ros::spin(); return 0; diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp index 974e88efa..23447cce6 100644 --- a/fuse_optimizers/src/windowed_optimizer.cpp +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -48,7 +48,6 @@ #include #include - namespace { /** @@ -77,18 +76,17 @@ typename std::vector::reverse_iterator erase( namespace fuse_optimizers { - WindowedOptimizer::WindowedOptimizer( fuse_core::Graph::UniquePtr graph, - const ParameterType& params, + const ParameterType::SharedPtr& params, const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) : - fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), - params_(params), - ignited_(false), - optimization_running_(true), - started_(false), - optimization_request_(false) + fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), + params_(params), + ignited_(false), + optimization_running_(true), + started_(false), + optimization_request_(false) { // Test for auto-start autostart(); @@ -97,14 +95,12 @@ WindowedOptimizer::WindowedOptimizer( optimization_thread_ = std::thread(&WindowedOptimizer::optimizationLoop, this); // Configure a timer to trigger optimizations - optimize_timer_ = node_handle_.createTimer( - params_.optimization_period, - &WindowedOptimizer::optimizerTimerCallback, - this); + optimize_timer_ = + node_handle_.createTimer(params_->optimization_period, &WindowedOptimizer::optimizerTimerCallback, this); // Advertise a service that resets the optimizer to its initial state reset_service_server_ = node_handle_.advertiseService( - ros::names::resolve(params_.reset_service), + ros::names::resolve(params_->reset_service), &WindowedOptimizer::resetServiceCallback, this); } @@ -123,8 +119,10 @@ WindowedOptimizer::~WindowedOptimizer() void WindowedOptimizer::autostart() { - if (std::none_of(sensor_models_.begin(), sensor_models_.end(), - [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) + if (std::none_of( + sensor_models_.begin(), + sensor_models_.end(), + [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) { // No ignition sensors were provided. Auto-start. started_ = true; @@ -188,14 +186,16 @@ void WindowedOptimizer::optimizationLoop() oss << "\nTransaction:\n"; new_transaction->print(oss); - ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() - << "\nLeaving optimization loop and requesting " - "node shutdown...\n" << oss.str()); + ROS_FATAL_STREAM( + "Failed to update graph with transaction: " << ex.what() + << "\nLeaving optimization loop and requesting " + "node shutdown...\n" + << oss.str()); ros::requestShutdown(); break; } // Optimize the entire graph - summary_ = graph_->optimize(params_.solver_options); + summary_ = graph_->optimize(params_->solver_options); // Optimization is complete. Notify all the things about the graph changes. const auto new_transaction_stamp = new_transaction->stamp(); @@ -204,8 +204,9 @@ void WindowedOptimizer::optimizationLoop() // Abort if optimization failed. Not converging is not a failure because the solution found is usable. if (!summary_.IsSolutionUsable()) { - ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + ROS_FATAL_STREAM( + "Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); ROS_INFO_STREAM(summary_.FullReport()); ros::requestShutdown(); break; @@ -214,10 +215,8 @@ void WindowedOptimizer::optimizationLoop() // Marginal out any variables outside of the current "window" // Determination of which variables to marginalize is delegated to derived classes auto variables_to_marginalize = computeVariablesToMarginalize(); - marginal_transaction_ = fuse_constraints::marginalizeVariables( - ros::this_node::getName(), - variables_to_marginalize, - *graph_); + marginal_transaction_ = + fuse_constraints::marginalizeVariables(ros::this_node::getName(), variables_to_marginalize, *graph_); // Perform any post-marginal cleanup -- Delegated to derived classes postprocessMarginalization(marginal_transaction_); // Note: The marginal transaction will not be applied until the next optimization iteration @@ -225,8 +224,10 @@ void WindowedOptimizer::optimizationLoop() auto optimization_complete = ros::Time::now(); if (optimization_complete > optimization_deadline) { - ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " - << (optimization_complete - optimization_deadline) << "s"); + ROS_WARN_STREAM_THROTTLE( + 10.0, + "Optimization exceeded the configured duration by " << (optimization_complete - optimization_deadline) + << "s"); } } } @@ -252,7 +253,7 @@ void WindowedOptimizer::optimizerTimerCallback(const ros::TimerEvent& event) { std::lock_guard lock(optimization_requested_mutex_); optimization_request_ = true; - optimization_deadline_ = event.current_expected + params_.optimization_period; + optimization_deadline_ = event.current_expected + params_->optimization_period; } optimization_requested_.notify_one(); } @@ -289,9 +290,10 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) { // We just started, but the oldest transaction is not from an ignition sensor. We will still process the // transaction, but we do not enforce it is processed individually. - ROS_ERROR_STREAM("The queued transaction with timestamp " << element.stamp() << " from sensor " << - element.sensor_name << " is not an ignition sensor transaction. " << - "This transaction will not be processed individually."); + ROS_ERROR_STREAM( + "The queued transaction with timestamp " << element.stamp() << " from sensor " << element.sensor_name + << " is not an ignition sensor transaction. " + << "This transaction will not be processed individually."); } else { @@ -306,18 +308,21 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) { // The motion model processing failed. When this happens to an ignition sensor transaction there is no point on // trying again next time, so we ignore this transaction. - ROS_ERROR_STREAM("The queued ignition transaction with timestamp " << element.stamp() << " from sensor " << - element.sensor_name << " could not be processed. Ignoring this ignition transaction."); + ROS_ERROR_STREAM( + "The queued ignition transaction with timestamp " << element.stamp() << " from sensor " << element.sensor_name + << " could not be processed. Ignoring this ignition " + "transaction."); // Remove the ignition transaction that just failed and purge all transactions after it. But if we find another // ignition transaction, we schedule it to be processed in the next optimization cycle. erase(pending_transactions_, transaction_rbegin); - const auto pending_ignition_transaction_iter = - std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), - [this](const auto& element) { // NOLINT(whitespace/braces) - return sensor_models_.at(element.sensor_name).ignition; - }); // NOLINT(whitespace/braces) + const auto pending_ignition_transaction_iter = std::find_if( + pending_transactions_.rbegin(), + pending_transactions_.rend(), + [this](const auto& element) { // NOLINT(whitespace/braces) + return sensor_models_.at(element.sensor_name).ignition; + }); // NOLINT(whitespace/braces) if (pending_ignition_transaction_iter == pending_transactions_.rend()) { // There is no other ignition transaction pending. We simply roll back to not started state and all other @@ -348,19 +353,11 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) while (transaction_riter != pending_transactions_.rend()) { auto& element = *transaction_riter; -/// @todo(swilliams) This is fairly specific to the fixed-lag smoother. I need to think of a way to generalize this -/// operation and push the implementation into the derived class. Maybe a filterQueuedTransactions() virtual method? -// const auto& min_stamp = element.minStamp(); -// if (min_stamp < lag_expiration) -// { -// ROS_DEBUG_STREAM("The current lag expiration time is " << lag_expiration << ". The queued transaction with " -// "timestamp " << element.stamp() << " from sensor " << element.sensor_name << " has a minimum " -// "involved timestamp of " << min_stamp << ", which is " << (lag_expiration - min_stamp) << -// " seconds too old. Ignoring this transaction."); -// transaction_riter = erase(pending_transactions_, transaction_riter); -// } -// else if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) - if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) + if (!validateTransaction(element.sensor_name, *element.transaction)) + { + transaction_riter = erase(pending_transactions_, transaction_riter); + } + else if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) { // We should not process transactions from this sensor ++transaction_riter; @@ -376,14 +373,18 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) // The motion model processing failed. // Check the transaction timeout to determine if it should be removed or skipped. const auto& max_stamp = element.maxStamp(); - if (max_stamp + params_.transaction_timeout < current_time) + if (max_stamp + params_->transaction_timeout < current_time) { // Warn that this transaction has expired, then skip it. - ROS_ERROR_STREAM("The queued transaction with timestamp " << element.stamp() << " and maximum " - "involved stamp of " << max_stamp << " from sensor " << element.sensor_name << - " could not be processed after " << (current_time - max_stamp) << " seconds, " - "which is greater than the 'transaction_timeout' value of " << - params_.transaction_timeout << ". Ignoring this transaction."); + ROS_ERROR_STREAM( + "The queued transaction with timestamp " << element.stamp() + << " and maximum " + "involved stamp of " + << max_stamp << " from sensor " << element.sensor_name + << " could not be processed after " << (current_time - max_stamp) + << " seconds, " + "which is greater than the 'transaction_timeout' value of " + << params_->transaction_timeout << ". Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); } else @@ -441,9 +442,11 @@ void WindowedOptimizer::transactionCallback( const auto max_time = transaction->maxStamp(); if (started_ && max_time < start_time) { - ROS_DEBUG_STREAM("Received a transaction before the start time from sensor '" << sensor_name << "'.\n" << - " start_time: " << start_time << ", maximum involved stamp: " << max_time << - ", difference: " << (start_time - max_time) << "s"); + ROS_DEBUG_STREAM( + "Received a transaction before the start time from sensor '" + << sensor_name << "'.\n" + << " start_time: " << start_time << ", maximum involved stamp: " << max_time + << ", difference: " << (start_time - max_time) << "s"); return; } { @@ -456,12 +459,9 @@ void WindowedOptimizer::transactionCallback( { return value >= element.stamp(); }; - auto position = std::upper_bound( - pending_transactions_.begin(), - pending_transactions_.end(), - transaction->stamp(), - comparator); - position = pending_transactions_.insert(position, {sensor_name, std::move(transaction)}); // NOLINT + auto position = + std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); + position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT // If we haven't "started" yet.. if (!started_) @@ -478,25 +478,28 @@ void WindowedOptimizer::transactionCallback( // - Either before or exactly at the start time // - Or with a minimum time before the minimum time of this ignition sensor transaction // - // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we can use + // TODO(efernandez) Do '&min_time = std::as_const(start_time)' when C++17 is supported and we can use // std::as_const: https://en.cppreference.com/w/cpp/utility/as_const pending_transactions_.erase( - std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); + std::remove_if( + pending_transactions_.begin(), + pending_transactions_.end(), + [&sensor_name, + max_time, + &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); } else { // And purge out old transactions to limit the pending size while waiting for an ignition sensor auto purge_time = ros::Time(0, 0); auto last_pending_time = pending_transactions_.front().stamp(); - if (ros::Time(0, 0) + params_.transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives + if (ros::Time(0, 0) + params_->transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives { - purge_time = last_pending_time - params_.transaction_timeout; + purge_time = last_pending_time - params_->transaction_timeout; } while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) @@ -603,7 +606,7 @@ void WindowedOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapp if (!optimization_deadline.isZero()) // This is zero for the default-constructed optimization_deadline object { - const auto optimization_request_time = optimization_deadline - params_.optimization_period; + const auto optimization_request_time = optimization_deadline - params_->optimization_period; const auto time_since_last_optimization_request = ros::Time::now() - optimization_request_time; status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.toSec()); } From e37da706bfe074d90c9a0da67a7c51cb19563103 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 6 Dec 2021 12:36:18 -0500 Subject: [PATCH 05/17] Initial fixed size smoother implementation --- fuse_optimizers/CMakeLists.txt | 2 + .../fuse_optimizers/variable_stamp_index.h | 18 ++++ fuse_optimizers/src/variable_stamp_index.cpp | 42 +++++--- .../test/test_variable_stamp_index.cpp | 96 +++++++++++++++++++ 4 files changed, 147 insertions(+), 11 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index e8a1fa34b..147621c17 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") + set(build_depends diagnostic_updater fuse_constraints diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h index f0342f9dd..59f5a186d 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h @@ -78,6 +78,11 @@ class VariableStampIndex */ size_t size() const { return variables_.size(); } + /** + * @brief Returns the number of stamped robot states + */ + size_t numStates() const { return unique_stamps_.size(); } + /** * @brief Clear all tracked state */ @@ -86,6 +91,7 @@ class VariableStampIndex stamped_index_.clear(); variables_.clear(); constraints_.clear(); + unique_stamps_.clear(); } /** @@ -93,6 +99,16 @@ class VariableStampIndex */ ros::Time currentStamp() const; + /** + * @brief Returns the oldest/first stamp + */ + ros::Time firstStamp() const; + + /** + * @brief Returns the ith timestamp + */ + ros::Time ithStamp(size_t idx) const; + /** * @brief Update the index with the information from the added transactions * @@ -176,6 +192,8 @@ class VariableStampIndex using ConstraintToVariablesMap = std::unordered_map>; ConstraintToVariablesMap constraints_; + std::set unique_stamps_; //!< This hold all the unique stamps to track total number of robot "states" + /** * @brief Update this VariableStampIndex with the added constraints from the provided transaction */ diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index d3127b5da..d8b63bded 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -47,19 +47,29 @@ namespace fuse_optimizers { ros::Time VariableStampIndex::currentStamp() const { - auto compare_stamps = [](const StampedMap::value_type& lhs, const StampedMap::value_type& rhs) - { - return lhs.second < rhs.second; - }; - auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps); - if (iter != stamped_index_.end()) - { - return iter->second; + if(!unique_stamps_.empty()){ + return *unique_stamps_.end(); } - else - { - return ros::Time(0, 0); + return ros::Time(0, 0); +} + +ros::Time VariableStampIndex::firstStamp() const +{ + if(!unique_stamps_.empty()){ + return *unique_stamps_.begin(); } + return ros::Time(0, 0); +} + +ros::Time VariableStampIndex::ithStamp(size_t idx) const{ + if(idx < unique_stamps_.size()){ + std::set::iterator it = unique_stamps_.begin(); + for(size_t i = 0; i < idx; i++){ + it++; + } + return *(it); + } + return ros::Time(0, 0); } void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transaction) @@ -99,6 +109,10 @@ void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& trans if (stamped_variable) { stamped_index_[variable.uuid()] = stamped_variable->stamp(); + // add to unique stamps + if(unique_stamps_.find(stamped_variable->stamp()) == unique_stamps_.end()){ + unique_stamps_.insert(stamped_variable->stamp()); + } } variables_[variable.uuid()]; // Add an empty set of constraints } @@ -120,6 +134,12 @@ void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction& tra { for (const auto& variable_uuid : transaction.removedVariables()) { + // remove from unique stamps + auto stamp = stamped_index_[variable_uuid]; + if(unique_stamps_.find(stamp) != unique_stamps_.end()){ + unique_stamps_.erase(stamp); + } + stamped_index_.erase(variable_uuid); variables_.erase(variable_uuid); } diff --git a/fuse_optimizers/test/test_variable_stamp_index.cpp b/fuse_optimizers/test/test_variable_stamp_index.cpp index 5003d3646..2fd9e8be3 100644 --- a/fuse_optimizers/test/test_variable_stamp_index.cpp +++ b/fuse_optimizers/test/test_variable_stamp_index.cpp @@ -282,6 +282,102 @@ TEST(VariableStampIndex, CurrentStamp) EXPECT_EQ(ros::Time(1, 0), index.currentStamp()); } +TEST(VariableStampIndex, FirstStamp) +{ + // Create an empty index + auto index = fuse_optimizers::VariableStampIndex(); + + // Verify the current stamp is 0 + EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + + // Add an unstamped variable + auto x1 = UnstampedVariable::make_shared(); + auto transaction1 = fuse_core::Transaction(); + transaction1.addVariable(x1); + index.addNewTransaction(transaction1); + + // Verify the current stamp is still 0 + EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + + // Add two stamped variables + auto x2 = StampedVariable::make_shared(ros::Time(1, 0)); + auto transaction2 = fuse_core::Transaction(); + transaction2.addVariable(x2); + index.addNewTransaction(transaction2); + + auto x3 = StampedVariable::make_shared(ros::Time(2, 0)); + auto transaction3 = fuse_core::Transaction(); + transaction3.addVariable(x3); + index.addNewTransaction(transaction3); + + // Verify the current stamp is now Time(1, 0) + EXPECT_EQ(ros::Time(1, 0), index.firstStamp()); +} + +TEST(VariableStampIndex, NumSates) +{ + // Create an empty index + auto index = fuse_optimizers::VariableStampIndex(); + + // Verify the current stamp is 0 + EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + + // Add an unstamped variable + auto x1 = UnstampedVariable::make_shared(); + auto transaction1 = fuse_core::Transaction(); + transaction1.addVariable(x1); + index.addNewTransaction(transaction1); + + // Verify the current stamp is still 0 + EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + + // Add N stamped variables + int N = 10; + + for(int i = 0; i < N; i++){ + auto x = StampedVariable::make_shared(ros::Time(i, 0)); + auto transaction = fuse_core::Transaction(); + transaction.addVariable(x); + index.addNewTransaction(transaction); + } + + // Verify the number of unique stamps + EXPECT_EQ((size_t)N, index.numStates()); +} + + +TEST(VariableStampIndex, ithStamp) +{ + // Create an empty index + auto index = fuse_optimizers::VariableStampIndex(); + + // Verify the current stamp is 0 + EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + + // Add an unstamped variable + auto x1 = UnstampedVariable::make_shared(); + auto transaction1 = fuse_core::Transaction(); + transaction1.addVariable(x1); + index.addNewTransaction(transaction1); + + // Verify the current stamp is still 0 + EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + + // Add N stamped variables + int N = 10; + for(int i = 0; i < N; i++){ + auto x = StampedVariable::make_shared(ros::Time(i, 0)); + auto transaction = fuse_core::Transaction(); + transaction.addVariable(x); + index.addNewTransaction(transaction); + } + + // Verify the stamps of the states using ithStamp function + for(int i = 0; i < N; i++){ + EXPECT_EQ(ros::Time(i, 0), index.ithStamp((size_t)i)); + } +} + TEST(VariableStampIndex, Query) { // Create an empty index From 25c3958e36b6176f4098a3d462197746dc3dcaf1 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 6 Dec 2021 12:36:23 -0500 Subject: [PATCH 06/17] Initial fixed size smoother implementation --- .vscode/settings.json | 86 ++++++++ .../fuse_optimizers/fixed_size_smoother.h | 197 ++++++++++++++++++ .../fixed_size_smoother_params.h | 80 +++++++ fuse_optimizers/src/fixed_size_smoother.cpp | 128 ++++++++++++ 4 files changed, 491 insertions(+) create mode 100644 .vscode/settings.json create mode 100644 fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h create mode 100644 fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h create mode 100644 fuse_optimizers/src/fixed_size_smoother.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 000000000..7e678cff8 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,86 @@ +{ + "files.associations": { + "functional": "cpp", + "__locale": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "__bit_reference": "cpp", + "__config": "cpp", + "__debug": "cpp", + "__functional_base": "cpp", + "__hash_table": "cpp", + "__mutex_base": "cpp", + "__nullptr": "cpp", + "__split_buffer": "cpp", + "__string": "cpp", + "__threading_support": "cpp", + "__tree": "cpp", + "__tuple": "cpp", + "ios": "cpp", + "locale": "cpp", + "queue": "cpp", + "stack": "cpp" + } +} \ No newline at end of file diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h new file mode 100644 index 000000000..3cbe6d041 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h @@ -0,0 +1,197 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_H +#define FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace fuse_optimizers +{ +/** + * @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time + * + * This implementation assumes that all added variable types are either derived from the fuse_variables::Stamped class, + * or are directly connected to at least one fuse_variables::Stamped variable via a constraint. The current time of + * the fixed-Size smoother is determined by the newest stamp of all added fuse_variables::Stamped variables. + * + * During optimization: + * (1) new variables and constraints are added to the graph + * (2) the augmented graph is optimized and the variable values are updated + * (3) all motion models, sensors, and publishers are notified of the updated graph + * (4) all variables older than "current time - Size duration" are marginalized out. + * + * Optimization is performed at a fixed frequency, controlled by the \p optimization_frequency parameter. Received + * sensor transactions are queued while the optimization is processing, then applied to the graph at the start of the + * next optimization cycle. If the previous optimization is not yet complete when the optimization period elapses, + * then a warning will be logged but a new optimization will *not* be started. The previous optimization will run to + * completion, and the next optimization will not begin until the next scheduled optimization period. + * + * Parameters: + * - Size_duration (float, default: 5.0) The duration of the smoothing window in seconds + * - motion_models (struct array) The set of motion model plugins to load + * @code{.yaml} + * - name: string (A unique name for this motion model) + * type: string (The plugin loader class string for the desired motion model type) + * - ... + * @endcode + * - optimization_frequency (float, default: 10.0) The target frequency for optimization cycles. If an optimization + * takes longer than expected, an optimization cycle may be skipped. + * - publishers (struct array) The set of publisher plugins to load + * @code{.yaml} + * - name: string (A unique name for this publisher) + * type: string (The plugin loader class string for the desired publisher type) + * - ... + * @endcode + * - sensor_models (struct array) The set of sensor model plugins to load + * @code{.yaml} + * - name: string (A unique name for this sensor model) + * type: string (The plugin loader class string for the desired sensor model type) + * motion_models: [name1, name2, ...] (An optional list of motion model names that should be applied) + * - ... + * @endcode + * - transaction_timeout (float, default: 0.10) The maximum time to wait for motion models to be generated for a + * received transactions. Transactions are processes sequentially, so + * no new transactions will be added to the graph while waiting for + * motion models to be generated. Once the timeout expires, that + * transaction will be deleted from the queue. + */ +class FixedSizeSmoother : public WindowedOptimizer +{ +public: + SMART_PTR_DEFINITIONS(FixedSizeSmoother); + using ParameterType = FixedSizeSmootherParams; + + /** + * @brief Constructor + * + * @param[in] graph The derived graph object. This allows different graph implementations to be used + * with the same optimizer code. + * @param[in] params A structure containing all of the configuration parameters required by the + * fixed-size smoother + * @param[in] node_handle A node handle in the global namespace + * @param[in] private_node_handle A node handle in the node's private namespace + */ + FixedSizeSmoother( + fuse_core::Graph::UniquePtr graph, + const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + + /** + * @brief Constructor + * + * The parameters will be loaded from the ROS parameter server using the private node handle + * + * @param[in] graph The derived graph object. This allows different graph implementations to be used + * with the same optimizer code. + * @param[in] node_handle A node handle in the global namespace + * @param[in] private_node_handle A node handle in the node's private namespace + */ + explicit FixedSizeSmoother( + fuse_core::Graph::UniquePtr graph, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + +protected: + // Read-only after construction + ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-Size smoother + + // Guarded by mutex_ + std::mutex mutex_; //!< Mutex held while the fixed-size smoother variables are modified + VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable + + /** + * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called + * + * All new transactions that will be applied to the graph are provided. This does not include the marginal + * transaction that is computed later. + * + * This method will be called before the graph has been updated. + * + * @param[in] new_transaction All new, non-marginal-related transactions that *will be* applied to the graph + */ + void preprocessMarginalization(const fuse_core::Transaction& new_transaction) override; + + /** + * @brief Compute the set of variables that should be marginalized from the graph + * + * This will be called after \p preprocessMarginalization() and after the graph has been updated with the any + * previous marginal transactions and new transactions. + * + * @param[in] Size_expiration The oldest timestamp that should remain in the graph + * @return A container with the set of variables to marginalize out. Order of the variables is not specified. + */ + std::vector computeVariablesToMarginalize() override; + + /** + * @brief Perform any required post-marginalization bookkeeping + * + * The transaction containing the actual changed to the graph is supplied. This will be called before the + * transaction is actually applied to the graph. + * + * @param[in] marginal_transaction The actual changes to the graph caused my marginalizing out the requested + * variables. + */ + void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) override; + + /** + * @brief Determine if a new transaction should be applied to the graph + * + * Test if the transaction is within the defined Size window of the smoother. + * + * @param[in] sensor_name - The name of the sensor that produced the provided transaction + * @param[in] transaction - The transaction to be validated + */ + bool validateTransaction(const std::string& sensor_name, const fuse_core::Transaction& transaction) override; + + /** + * @brief Perform any required operations whenever the optimizer is reset + */ + void onReset() override; +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_H diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h new file mode 100644 index 000000000..b54aa97d5 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h @@ -0,0 +1,80 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H +#define FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace fuse_optimizers +{ +/** + * @brief Defines the set of parameters required by the fuse_optimizers::FixedSizeSmoother class + */ +struct FixedSizeSmootherParams : public WindowedOptimizerParams +{ +public: + SMART_PTR_DEFINITIONS(FixedSizeSmootherParams); + + /** + * @brief The duration of the smoothing window in seconds + */ + size_t num_states {10}; + + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) + { + WindowedOptimizerParams::loadFromROS(nh); + + // Read settings from the parameter server + fuse_core::getPositiveParam(nh, "num_states", num_states); + } +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H diff --git a/fuse_optimizers/src/fixed_size_smoother.cpp b/fuse_optimizers/src/fixed_size_smoother.cpp new file mode 100644 index 000000000..d50c32724 --- /dev/null +++ b/fuse_optimizers/src/fixed_size_smoother.cpp @@ -0,0 +1,128 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace fuse_optimizers +{ +FixedSizeSmoother::FixedSizeSmoother( + fuse_core::Graph::UniquePtr graph, + const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) : + fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), + params_(params) +{ +} + +FixedSizeSmoother::FixedSizeSmoother( + fuse_core::Graph::UniquePtr graph, + const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) : + FixedSizeSmoother::FixedSizeSmoother( + std::move(graph), + ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), + node_handle, + private_node_handle) +{ +} + +void FixedSizeSmoother::preprocessMarginalization(const fuse_core::Transaction& new_transaction) +{ + std::lock_guard lock(mutex_); + timestamp_tracking_.addNewTransaction(new_transaction); +} + +std::vector FixedSizeSmoother::computeVariablesToMarginalize() +{ + std::lock_guard lock(mutex_); + + auto marginalize_variable_uuids = std::vector(); + + // if the total number of states is greater than our optimization window, then find the new state + // given we remove the first n states to bring the number of states back to our desired window size + if(timestamp_tracking_.numStates() > params_.num_states){ + size_t num_states_to_marginalize = timestamp_tracking_.numStates() - params_.num_states; + ros::Time new_start_time = timestamp_tracking_.ithStamp(num_states_to_marginalize - 1); + timestamp_tracking_.query(new_start_time, std::back_inserter(marginalize_variable_uuids)); + } + + return marginalize_variable_uuids; +} + +void FixedSizeSmoother::postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) +{ + std::lock_guard lock(mutex_); + timestamp_tracking_.addMarginalTransaction(marginal_transaction); +} + +bool FixedSizeSmoother::validateTransaction(const std::string& sensor_name, const fuse_core::Transaction& transaction) +{ + auto min_stamp = transaction.minStamp(); + std::lock_guard lock(mutex_); + ros::Time window_start = timestamp_tracking_.firstStamp(); + if (min_stamp < window_start) + { + ROS_DEBUG_STREAM( + "The current optimization window starts at " + << window_start << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " + << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is prior to the beginning " + << "of this window, ignoring this transaction."); + return false; + } + return true; +} + +void FixedSizeSmoother::onReset() +{ + std::lock_guard lock(mutex_); + timestamp_tracking_.clear(); + Size_expiration_ = ros::Time(0, 0); +} + +} // namespace fuse_optimizers From bc5c21e90f6ab0d8dcc2cba5ef83ef8a5ec17273 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 6 Dec 2021 12:58:25 -0500 Subject: [PATCH 07/17] Add fixed size smoother node --- fuse_optimizers/CMakeLists.txt | 22 ++++++++ .../src/fixed_size_smoother_node.cpp | 51 +++++++++++++++++++ 2 files changed, 73 insertions(+) create mode 100644 fuse_optimizers/src/fixed_size_smoother_node.cpp diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 147621c17..22fe18822 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -102,6 +102,28 @@ set_target_properties(fixed_lag_smoother_node CXX_STANDARD_REQUIRED YES ) +## fixed_size_smoother node +add_executable(fixed_size_smoother_node + src/fixed_size_smoother_node.cpp +) +add_dependencies(fixed_size_smoother_node + ${catkin_EXPORTED_TARGETS} +) +target_include_directories(fixed_size_smoother_node + PRIVATE + include + ${catkin_INCLUDE_DIRS} +) +target_link_libraries(fixed_size_smoother_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(fixed_size_smoother_node + PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED YES +) + ############# ## Install ## ############# diff --git a/fuse_optimizers/src/fixed_size_smoother_node.cpp b/fuse_optimizers/src/fixed_size_smoother_node.cpp new file mode 100644 index 000000000..2e7c65ce3 --- /dev/null +++ b/fuse_optimizers/src/fixed_size_smoother_node.cpp @@ -0,0 +1,51 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include +#include + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "fixed_size_smoother_node"); + ros::NodeHandle private_node_handle("~"); + fuse_graphs::HashGraphParams hash_graph_params; + hash_graph_params.loadFromROS(private_node_handle); + fuse_optimizers::FixedSizeSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); + ros::spin(); + + return 0; +} From 1d88752b0b17494433500b301f194775696f1cd5 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 6 Dec 2021 12:59:13 -0500 Subject: [PATCH 08/17] Remove temp compile flag --- fuse_optimizers/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 22fe18822..c4f5a8b19 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -2,8 +2,6 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") - set(build_depends diagnostic_updater fuse_constraints From 68b17d8b5a5a9186d3a0bc13e4653c2a33f4989b Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 6 Dec 2021 12:59:57 -0500 Subject: [PATCH 09/17] Remove vscode folder --- .vscode/settings.json | 86 ------------------------------------------- 1 file changed, 86 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 7e678cff8..000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,86 +0,0 @@ -{ - "files.associations": { - "functional": "cpp", - "__locale": "cpp", - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "strstream": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "complex": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "list": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "iterator": "cpp", - "map": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "set": "cpp", - "string": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "hash_map": "cpp", - "hash_set": "cpp", - "fstream": "cpp", - "future": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "ostream": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cfenv": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "__bit_reference": "cpp", - "__config": "cpp", - "__debug": "cpp", - "__functional_base": "cpp", - "__hash_table": "cpp", - "__mutex_base": "cpp", - "__nullptr": "cpp", - "__split_buffer": "cpp", - "__string": "cpp", - "__threading_support": "cpp", - "__tree": "cpp", - "__tuple": "cpp", - "ios": "cpp", - "locale": "cpp", - "queue": "cpp", - "stack": "cpp" - } -} \ No newline at end of file From febdb5fb8ad571bda97e2f013522617bba3e18fc Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 3 Jan 2022 12:04:07 -0500 Subject: [PATCH 10/17] fix compile errors --- fuse_optimizers/CMakeLists.txt | 1 + .../include/fuse_optimizers/fixed_size_smoother_params.h | 2 +- fuse_optimizers/src/fixed_size_smoother.cpp | 5 ++--- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index c4f5a8b19..7ed9a6e16 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -35,6 +35,7 @@ add_compile_options(-Wall -Werror) add_library(${PROJECT_NAME} src/batch_optimizer.cpp src/fixed_lag_smoother.cpp + src/fixed_size_smoother.cpp src/optimizer.cpp src/windowed_optimizer.cpp src/variable_stamp_index.cpp diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h index b54aa97d5..b8e208ede 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h @@ -59,7 +59,7 @@ struct FixedSizeSmootherParams : public WindowedOptimizerParams /** * @brief The duration of the smoothing window in seconds */ - size_t num_states {10}; + int num_states {10}; /** * @brief Method for loading parameter values from ROS. diff --git a/fuse_optimizers/src/fixed_size_smoother.cpp b/fuse_optimizers/src/fixed_size_smoother.cpp index d50c32724..4fea8b83d 100644 --- a/fuse_optimizers/src/fixed_size_smoother.cpp +++ b/fuse_optimizers/src/fixed_size_smoother.cpp @@ -86,8 +86,8 @@ std::vector FixedSizeSmoother::computeVariablesToMarginalize() // if the total number of states is greater than our optimization window, then find the new state // given we remove the first n states to bring the number of states back to our desired window size - if(timestamp_tracking_.numStates() > params_.num_states){ - size_t num_states_to_marginalize = timestamp_tracking_.numStates() - params_.num_states; + if((int)timestamp_tracking_.numStates() > params_->num_states){ + size_t num_states_to_marginalize = timestamp_tracking_.numStates() - params_->num_states; ros::Time new_start_time = timestamp_tracking_.ithStamp(num_states_to_marginalize - 1); timestamp_tracking_.query(new_start_time, std::back_inserter(marginalize_variable_uuids)); } @@ -122,7 +122,6 @@ void FixedSizeSmoother::onReset() { std::lock_guard lock(mutex_); timestamp_tracking_.clear(); - Size_expiration_ = ros::Time(0, 0); } } // namespace fuse_optimizers From caeb2781a5bb8f49e161c9eb3ae84c683e4677e0 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 5 Jan 2022 15:14:41 -0500 Subject: [PATCH 11/17] Fix marginalization bug --- fuse_optimizers/CMakeLists.txt | 2 ++ fuse_optimizers/src/windowed_optimizer.cpp | 34 ++++++++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 7ed9a6e16..c540d066d 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") + set(build_depends diagnostic_updater fuse_constraints diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp index 23447cce6..d1b4f7868 100644 --- a/fuse_optimizers/src/windowed_optimizer.cpp +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -173,6 +173,40 @@ void WindowedOptimizer::optimizationLoop() preprocessMarginalization(*new_transaction); // Combine the new transactions with any marginal transaction from the end of the last cycle new_transaction->merge(marginal_transaction_); + + // check for hanging constraints (constraints added to variables that are about to be marginalized) + auto new_marginal_transaction = fuse_core::Transaction::make_shared(); + std::vector variables_to_remarginalize; + for (auto &uuid : marginal_transaction_->removedVariables()) { + // get constraints that are to be removed in marginal transaction + auto to_remove_constraints = marginal_transaction_->getRemovedConstraints(); + // get connected constraint to this variable + auto all_constraints = graph_->getConnectedConstraints(uuid); + // check if there is a constraint on this variable that isnt in the + // marginal transaction + for (auto &constraint : all_constraints) { + fuse_core::UUID constraint_uuid = constraint->uuid(); + bool is_contained = false; + for (auto &to_remove : to_remove_constraints) { + if (constraint_uuid == to_remove) { + is_contained = true; + break; + } + } + if (!is_contained) { + variables_to_remarginalize.push_back(uuid); + } + } + } + + // if some variables have hanging constraints then remarginalize them + if (variables_to_remarginalize.size() > 0) { + new_marginal_transaction = fuse_constraints::marginalizeVariables( + ros::this_node::getName(), variables_to_remarginalize, *graph_); + postprocessMarginalization(*new_marginal_transaction); + new_transaction->merge(new_marginal_transaction); + } + // Update the graph try { From fce9dbd74b8658393374fc6c5e474d63070636ef Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Fri, 21 Jan 2022 14:16:02 -0600 Subject: [PATCH 12/17] Messy fix to marginalization issue --- fuse_optimizers/src/windowed_optimizer.cpp | 47 ++++++++-------------- 1 file changed, 16 insertions(+), 31 deletions(-) diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp index d1b4f7868..cbf2970da 100644 --- a/fuse_optimizers/src/windowed_optimizer.cpp +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -169,44 +169,29 @@ void WindowedOptimizer::optimizationLoop() { continue; } - // Prepare for selecting the marginal variables -- Delegated to derived classes - preprocessMarginalization(*new_transaction); - // Combine the new transactions with any marginal transaction from the end of the last cycle - new_transaction->merge(marginal_transaction_); - - // check for hanging constraints (constraints added to variables that are about to be marginalized) - auto new_marginal_transaction = fuse_core::Transaction::make_shared(); - std::vector variables_to_remarginalize; - for (auto &uuid : marginal_transaction_->removedVariables()) { - // get constraints that are to be removed in marginal transaction - auto to_remove_constraints = marginal_transaction_->getRemovedConstraints(); - // get connected constraint to this variable - auto all_constraints = graph_->getConnectedConstraints(uuid); - // check if there is a constraint on this variable that isnt in the - // marginal transaction - for (auto &constraint : all_constraints) { - fuse_core::UUID constraint_uuid = constraint->uuid(); - bool is_contained = false; - for (auto &to_remove : to_remove_constraints) { - if (constraint_uuid == to_remove) { - is_contained = true; + // check if new transaction has added constraints to variables that are to be marginalized + std::vector faulty_constraints; + for(auto& c: new_transaction->addedConstraints()){ + for(auto var_uuid: c.variables()){ + for (auto marginal_uuid : marginal_transaction_.removedVariables()) { + if (var_uuid == marginal_uuid) { + faulty_constraints.push_back(c.uuid()); break; } } - if (!is_contained) { - variables_to_remarginalize.push_back(uuid); - } } } - - // if some variables have hanging constraints then remarginalize them - if (variables_to_remarginalize.size() > 0) { - new_marginal_transaction = fuse_constraints::marginalizeVariables( - ros::this_node::getName(), variables_to_remarginalize, *graph_); - postprocessMarginalization(*new_marginal_transaction); - new_transaction->merge(new_marginal_transaction); + if(faulty_constraints.size() > 0){ + ROS_WARN_STREAM("Removing invalid constraints."); + for(auto& faulty_constraint: faulty_constraints){ + new_transaction->removeConstraint(faulty_constraint); + } } + // Prepare for selecting the marginal variables -- Delegated to derived classes + preprocessMarginalization(*new_transaction); + // Combine the new transactions with any marginal transaction from the end of the last cycle + new_transaction->merge(marginal_transaction_); // Update the graph try { From 002d6d1b5bff6861a4e184af21900d49559ca374 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 2 Feb 2022 14:06:02 -0600 Subject: [PATCH 13/17] Add reset message callback for resets called within other callbacks --- .../fuse_optimizers/windowed_optimizer.h | 7 ++++ fuse_optimizers/src/windowed_optimizer.cpp | 40 +++++++++++++++++++ 2 files changed, 47 insertions(+) diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h index 5211527d3..50332bdd6 100644 --- a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -184,6 +185,7 @@ class WindowedOptimizer : public Optimizer // Ordering ROS objects with callbacks last ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency ros::ServiceServer reset_service_server_; //!< Service that resets the optimizer to its initial state + ros::Subscriber reset_subscriber_; //!< Subscriber that resets the optimizer to its initial state /** * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called @@ -281,6 +283,11 @@ class WindowedOptimizer : public Optimizer */ void processQueue(fuse_core::Transaction& transaction); + /** + * @brief Message callback that resets the optimizer to its original state + */ + void resetMessageCallback(const std_msgs::Empty::ConstPtr&); + /** * @brief Service callback that resets the optimizer to its original state */ diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp index cbf2970da..b62c41932 100644 --- a/fuse_optimizers/src/windowed_optimizer.cpp +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -103,6 +103,12 @@ WindowedOptimizer::WindowedOptimizer( ros::names::resolve(params_->reset_service), &WindowedOptimizer::resetServiceCallback, this); + + // Subscribe to a reset message that will reset the optimizer to the initial state + reset_subscriber_ = node_handle_.subscribe( + ros::names::resolve(params_->reset_service), 10, + &WindowedOptimizer::resetMessageCallback, + this); } WindowedOptimizer::~WindowedOptimizer() @@ -416,6 +422,40 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) } } +void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&){ + // Tell all the plugins to stop + stopPlugins(); + // Reset the optimizer state + { + std::lock_guard lock(optimization_requested_mutex_); + optimization_request_ = false; + } + started_ = false; + ignited_ = false; + setStartTime(ros::Time(0, 0)); + // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the + // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to + // prevent the possibility of deadlocks. + { + std::lock_guard lock(optimization_mutex_); + // Clear all pending transactions + { + std::lock_guard lock(pending_transactions_mutex_); + pending_transactions_.clear(); + } + // Clear the graph and marginal tracking states + graph_->clear(); + marginal_transaction_ = fuse_core::Transaction(); + } + // Perform any required reset operations for derived classes + onReset(); + // Tell all the plugins to start + startPlugins(); + // Test for auto-start + autostart(); + return; +} + bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { // Tell all the plugins to stop From 157cd2dd6e24f11208d7d1866fbc3306af1b87a7 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 14 Feb 2022 18:11:23 -0500 Subject: [PATCH 14/17] Address PR comments --- fuse_optimizers/CMakeLists.txt | 8 +- .../include/fuse_optimizers/batch_optimizer.h | 33 ++- .../fuse_optimizers/batch_optimizer_params.h | 6 +- .../fuse_optimizers/fixed_lag_smoother.h | 18 +- .../fixed_lag_smoother_params.h | 2 +- .../fuse_optimizers/fixed_size_smoother.h | 26 +-- .../fixed_size_smoother_params.h | 14 +- .../include/fuse_optimizers/optimizer.h | 48 ++-- .../fuse_optimizers/variable_stamp_index.h | 34 +-- .../fuse_optimizers/windowed_optimizer.h | 36 ++- .../windowed_optimizer_params.h | 8 +- fuse_optimizers/src/batch_optimizer.cpp | 39 ++-- fuse_optimizers/src/batch_optimizer_node.cpp | 3 +- fuse_optimizers/src/fixed_lag_smoother.cpp | 35 +++ .../src/fixed_lag_smoother_node.cpp | 3 +- fuse_optimizers/src/fixed_size_smoother.cpp | 41 ++-- .../src/fixed_size_smoother_node.cpp | 5 +- fuse_optimizers/src/optimizer.cpp | 146 ++++++------ fuse_optimizers/src/variable_stamp_index.cpp | 60 ++--- fuse_optimizers/src/windowed_optimizer.cpp | 209 +++++++----------- fuse_optimizers/test/example_optimizer.h | 7 +- .../test/test_fixed_lag_ignition.cpp | 7 +- fuse_optimizers/test/test_optimizer.cpp | 29 ++- .../test/test_variable_stamp_index.cpp | 94 ++++---- 24 files changed, 428 insertions(+), 483 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index c540d066d..1f8bcad62 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -2,8 +2,6 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") - set(build_depends diagnostic_updater fuse_constraints @@ -15,6 +13,8 @@ set(build_depends std_srvs ) +find_package(OpenMP) + find_package(catkin REQUIRED COMPONENTS ${build_depends} ) @@ -52,12 +52,16 @@ target_include_directories(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} + ${OpenMP_CXX_LIBRARIES} ) set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 14 CXX_STANDARD_REQUIRED YES ) +target_compile_options(${PROJECT_NAME} + PRIVATE ${OpenMP_CXX_FLAGS} +) ## batch_optimizer node add_executable(batch_optimizer_node diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h index 1bab2c5a5..692d151d0 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h @@ -51,10 +51,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief A simple optimizer implementation that uses batch optimization * @@ -107,10 +105,8 @@ class BatchOptimizer : public Optimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - BatchOptimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + BatchOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Destructor @@ -126,11 +122,10 @@ class BatchOptimizer : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - TransactionQueueElement( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) : - sensor_name(sensor_name), - transaction(std::move(transaction)) {} + TransactionQueueElement(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) + : sensor_name(sensor_name), transaction(std::move(transaction)) + { + } }; /** @@ -145,19 +140,19 @@ class BatchOptimizer : public Optimizer fuse_core::Transaction::SharedPtr combined_transaction_; //!< Transaction used aggregate constraints and variables //!< from multiple sensors and motions models before being //!< applied to the graph. - std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction across different threads - ParameterType params_; //!< Configuration settings for this optimizer + std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction across different threads + ParameterType params_; //!< Configuration settings for this optimizer std::atomic optimization_request_; //!< Flag to trigger a new optimization std::condition_variable optimization_requested_; //!< Condition variable used by the optimization thread to wait //!< until a new optimization is requested by the main thread - std::mutex optimization_requested_mutex_; //!< Required condition variable mutex - std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process - ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency + std::mutex optimization_requested_mutex_; //!< Required condition variable mutex + std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process + ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency TransactionQueue pending_transactions_; //!< The set of received transactions that have not been added to the //!< optimizer yet. Transactions are added by the main thread, and removed //!< and processed by the optimization thread. std::mutex pending_transactions_mutex_; //!< Synchronize modification of the pending_transactions_ container - ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction + ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction bool started_; //!< Flag indicating the optimizer is ready/has received a transaction from an ignition sensor /** @@ -200,9 +195,7 @@ class BatchOptimizer : public Optimizer * to generate connected constraints. * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin */ - void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** * @brief Update and publish diagnotics diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h index cc92ee4c8..8dc632ef0 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h @@ -45,10 +45,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class */ @@ -62,7 +60,7 @@ struct BatchOptimizerParams * may be specified in either the "optimization_period" parameter in seconds, or in the "optimization_frequency" * parameter in Hz. */ - ros::Duration optimization_period { 0.1 }; + ros::Duration optimization_period{ 0.1 }; /** * @brief The maximum time to wait for motion models to be generated for a received transaction. @@ -70,7 +68,7 @@ struct BatchOptimizerParams * Transactions are processed sequentially, so no new transactions will be added to the graph while waiting for * motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue. */ - ros::Duration transaction_timeout { 0.1 }; + ros::Duration transaction_timeout{ 0.1 }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index 6f3e083f6..3df84e8e1 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -113,11 +113,9 @@ class FixedLagSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Constructor @@ -129,18 +127,16 @@ class FixedLagSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - explicit FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + explicit FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); protected: // Read-only after construction ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-lag smoother // Guarded by mutex_ - std::mutex mutex_; //!< Mutex held while the fixed-lag smoother variables are modified - ros::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window + std::mutex mutex_; //!< Mutex held while the fixed-lag smoother variables are modified + ros::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable /** diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h index da866f292..4e78734d1 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h @@ -59,7 +59,7 @@ struct FixedLagSmootherParams : public WindowedOptimizerParams /** * @brief The duration of the smoothing window in seconds */ - ros::Duration lag_duration { 5.0 }; + ros::Duration lag_duration{ 5.0 }; /** * @brief Method for loading parameter values from ROS. diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h index 3cbe6d041..7498dff19 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -50,17 +50,17 @@ namespace fuse_optimizers { /** - * @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time + * @brief A fixed-2ize smoother implementation that marginalizes out variables that are older than a defined buffer size * * This implementation assumes that all added variable types are either derived from the fuse_variables::Stamped class, * or are directly connected to at least one fuse_variables::Stamped variable via a constraint. The current time of - * the fixed-Size smoother is determined by the newest stamp of all added fuse_variables::Stamped variables. + * the fixed-size smoother is determined by the newest stamp of all added fuse_variables::Stamped variables. * * During optimization: * (1) new variables and constraints are added to the graph * (2) the augmented graph is optimized and the variable values are updated * (3) all motion models, sensors, and publishers are notified of the updated graph - * (4) all variables older than "current time - Size duration" are marginalized out. + * (4) all variables outside of the fixed buffer are marginalized out * * Optimization is performed at a fixed frequency, controlled by the \p optimization_frequency parameter. Received * sensor transactions are queued while the optimization is processing, then applied to the graph at the start of the @@ -69,7 +69,7 @@ namespace fuse_optimizers * completion, and the next optimization will not begin until the next scheduled optimization period. * * Parameters: - * - Size_duration (float, default: 5.0) The duration of the smoothing window in seconds + * - num_states (float, default: 10) The number of unique timestamped states in the window * - motion_models (struct array) The set of motion model plugins to load * @code{.yaml} * - name: string (A unique name for this motion model) @@ -113,11 +113,9 @@ class FixedSizeSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Constructor @@ -129,17 +127,15 @@ class FixedSizeSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - explicit FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + explicit FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); protected: // Read-only after construction ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-Size smoother // Guarded by mutex_ - std::mutex mutex_; //!< Mutex held while the fixed-size smoother variables are modified + std::mutex mutex_; //!< Mutex held while the fixed-size smoother variables are modified VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable /** diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h index b8e208ede..fb2c795a5 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,18 +34,10 @@ #ifndef FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H #define FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H -#include #include #include -#include #include -#include - -#include -#include -#include - namespace fuse_optimizers { /** @@ -57,9 +49,9 @@ struct FixedSizeSmootherParams : public WindowedOptimizerParams SMART_PTR_DEFINITIONS(FixedSizeSmootherParams); /** - * @brief The duration of the smoothing window in seconds + * @brief Thenumber of unique stamps in the window */ - int num_states {10}; + int num_states{ 10 }; /** * @brief Method for loading parameter values from ROS. diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index dadfc76a2..1dcf3d4bd 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -49,10 +49,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief A base class that can be used to build fuse optimizer nodes * @@ -103,10 +101,8 @@ class Optimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - Optimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + Optimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Destructor @@ -144,25 +140,25 @@ class Optimizer using SensorModels = std::unordered_map; // Some internal book-keeping data structures - using MotionModelGroup = std::vector; //!< A set of motion model names + using MotionModelGroup = std::vector; //!< A set of motion model names using AssociatedMotionModels = std::unordered_map; //!< sensor -> motion models group AssociatedMotionModels associated_motion_models_; //!< Tracks what motion models should be used for each sensor - fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and constraints + fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and constraints // Ordering ROS objects with callbacks last - ros::NodeHandle node_handle_; //!< Node handle in the public namespace for subscribing and advertising + ros::NodeHandle node_handle_; //!< Node handle in the public namespace for subscribing and advertising ros::NodeHandle private_node_handle_; //!< Node handle in the private namespace for reading configuration settings pluginlib::ClassLoader motion_model_loader_; //!< Pluginlib class loader for MotionModels - MotionModels motion_models_; //!< The set of motion models, addressable by name + MotionModels motion_models_; //!< The set of motion models, addressable by name pluginlib::ClassLoader publisher_loader_; //!< Pluginlib class loader for Publishers Publishers publishers_; //!< The set of publishers to execute after every graph optimization pluginlib::ClassLoader sensor_model_loader_; //!< Pluginlib class loader for SensorModels SensorModels sensor_models_; //!< The set of sensor models, addressable by name diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater - ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer - double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds + ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer + double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds /** * @brief Callback fired every time a SensorModel plugin creates a new transaction @@ -172,9 +168,7 @@ class Optimizer * to generate connected constraints. * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin */ - virtual void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) = 0; + virtual void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0; /** * @brief Configure the motion model plugins specified on the parameter server @@ -211,9 +205,7 @@ class Optimizer * models * @return Flag indicating if all motion model constraints were successfully generated */ - bool applyMotionModels( - const std::string& sensor_name, - fuse_core::Transaction& transaction) const; + bool applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const; /** * @brief Send the sensors, motion models, and publishers updated graph information @@ -221,19 +213,15 @@ class Optimizer * @param[in] transaction A read-only pointer to a transaction containing all recent additions and removals * @param[in] graph A read-only pointer to the graph object */ - void notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph); + void notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph); - /** - * @brief Inject a transaction callback function into the global callback queue - * - * @param[in] sensor_name The name of the sensor that produced the Transaction - * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin - */ - void injectCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction); + /** + * @brief Inject a transaction callback function into the global callback queue + * + * @param[in] sensor_name The name of the sensor that produced the Transaction + * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin + */ + void injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction); /** * @brief Clear all of the callbacks inserted into the callback queue by the injectCallback() method diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h index 59f5a186d..cb977921f 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h @@ -71,17 +71,26 @@ class VariableStampIndex /** * @brief Return true if no variables exist in the index */ - bool empty() const { return variables_.empty() && constraints_.empty(); } + bool empty() const + { + return variables_.empty() && constraints_.empty(); + } /** * @brief Returns the number of variables in the index */ - size_t size() const { return variables_.size(); } + size_t size() const + { + return variables_.size(); + } /** * @brief Returns the number of stamped robot states */ - size_t numStates() const { return unique_stamps_.size(); } + int numStates() const + { + return unique_stamps_.size(); + } /** * @brief Clear all tracked state @@ -95,19 +104,11 @@ class VariableStampIndex } /** - * @brief Returns the most recent timestamp associated with any variable - */ - ros::Time currentStamp() const; - - /** - * @brief Returns the oldest/first stamp - */ - ros::Time firstStamp() const; - - /** - * @brief Returns the ith timestamp + * @brief Provides a random access operator the the unique timestamps + * + * @param[in] i index to access, if negative Time(0) is returned */ - ros::Time ithStamp(size_t idx) const; + ros::Time operator[](int i); /** * @brief Update the index with the information from the added transactions @@ -192,7 +193,8 @@ class VariableStampIndex using ConstraintToVariablesMap = std::unordered_map>; ConstraintToVariablesMap constraints_; - std::set unique_stamps_; //!< This hold all the unique stamps to track total number of robot "states" + using StampToVariablesMap = std::map>; + StampToVariablesMap unique_stamps_; //!< This holds all the unique stamps to track total number of robot "states" /** * @brief Update this VariableStampIndex with the added constraints from the provided transaction diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h index 50332bdd6..6c4598204 100644 --- a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h @@ -112,11 +112,9 @@ class WindowedOptimizer : public Optimizer * @param[in] node_handle - A node handle in the global namespace * @param[in] private_node_handle - A node handle in the node's private namespace */ - WindowedOptimizer( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + WindowedOptimizer(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Destructor @@ -132,9 +130,18 @@ class WindowedOptimizer : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - const ros::Time& stamp() const { return transaction->stamp(); } - const ros::Time& minStamp() const { return transaction->minStamp(); } - const ros::Time& maxStamp() const { return transaction->maxStamp(); } + const ros::Time& stamp() const + { + return transaction->stamp(); + } + const ros::Time& minStamp() const + { + return transaction->minStamp(); + } + const ros::Time& maxStamp() const + { + return transaction->maxStamp(); + } }; /** @@ -174,18 +181,18 @@ class WindowedOptimizer : public Optimizer // Guarded by optimization_requested_mutex_ std::mutex optimization_requested_mutex_; //!< Required condition variable mutex ros::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers a warning if exceeded. - bool optimization_request_; //!< Flag to trigger a new optimization + bool optimization_request_; //!< Flag to trigger a new optimization std::condition_variable optimization_requested_; //!< Condition variable used by the optimization thread to wait //!< until a new optimization is requested by the main thread // Guarded by start_time_mutex_ mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable - ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction + ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction // Ordering ROS objects with callbacks last - ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency + ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency ros::ServiceServer reset_service_server_; //!< Service that resets the optimizer to its initial state - ros::Subscriber reset_subscriber_; //!< Subscriber that resets the optimizer to its initial state + ros::Subscriber reset_subscriber_; //!< Subscriber that resets the optimizer to its initial state /** * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called @@ -283,6 +290,11 @@ class WindowedOptimizer : public Optimizer */ void processQueue(fuse_core::Transaction& transaction); + /** + * @brief Performs all logic when the client calls for a reset + */ + void reset(); + /** * @brief Message callback that resets the optimizer to its original state */ diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h index 5ad4305c4..d7e58d502 100644 --- a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h @@ -60,12 +60,12 @@ struct WindowedOptimizerParams * may be specified in either the "optimization_period" parameter in seconds, or in the "optimization_frequency" * parameter in Hz. */ - ros::Duration optimization_period { 0.1 }; + ros::Duration optimization_period{ 0.1 }; /** * @brief The topic name of the advertised reset service */ - std::string reset_service { "~reset" }; + std::string reset_service{ "~reset" }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. @@ -78,7 +78,7 @@ struct WindowedOptimizerParams * Transactions are processed sequentially, so no new transactions will be added to the graph while waiting for * motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue. */ - ros::Duration transaction_timeout { 0.1 }; + ros::Duration transaction_timeout{ 0.1 }; /** * @brief Method for loading parameter values from ROS. @@ -90,7 +90,7 @@ struct WindowedOptimizerParams // Read settings from the parameter server if (nh.hasParam("optimization_frequency")) { - double optimization_frequency { 1.0 / optimization_period.toSec() }; + double optimization_frequency{ 1.0 / optimization_period.toSec() }; fuse_core::getPositiveParam(nh, "optimization_frequency", optimization_frequency); optimization_period.fromSec(1.0 / optimization_frequency); } diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index fb39aea39..35115d7b1 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -42,27 +42,21 @@ #include #include - namespace fuse_optimizers { - -BatchOptimizer::BatchOptimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), - combined_transaction_(fuse_core::Transaction::make_shared()), - optimization_request_(false), - start_time_(ros::TIME_MAX), - started_(false) +BatchOptimizer::BatchOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) + : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) + , combined_transaction_(fuse_core::Transaction::make_shared()) + , optimization_request_(false) + , start_time_(ros::TIME_MAX) + , started_(false) { params_.loadFromROS(private_node_handle); // Configure a timer to trigger optimizations - optimize_timer_ = node_handle_.createTimer( - ros::Duration(params_.optimization_period), - &BatchOptimizer::optimizerTimerCallback, - this); + optimize_timer_ = node_handle_.createTimer(ros::Duration(params_.optimization_period), + &BatchOptimizer::optimizerTimerCallback, this); // Start the optimization thread optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this); @@ -99,10 +93,11 @@ void BatchOptimizer::applyMotionModelsToQueue() if (element.transaction->stamp() + params_.transaction_timeout < current_time) { // Warn that this transaction has expired, then skip it. - ROS_ERROR_STREAM("The queued transaction with timestamp " << element.transaction->stamp() - << " could not be processed after " << (current_time - element.transaction->stamp()) - << " seconds, which is greater than the 'transaction_timeout' value of " - << params_.transaction_timeout << ". Ignoring this transaction."); + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.transaction->stamp() << " could not be processed after " + << (current_time - element.transaction->stamp()) + << " seconds, which is greater than the 'transaction_timeout' value of " + << params_.transaction_timeout << ". Ignoring this transaction."); pending_transactions_.erase(pending_transactions_.begin()); continue; } @@ -130,7 +125,7 @@ void BatchOptimizer::optimizationLoop() // Wait for the next signal to start the next optimization cycle { std::unique_lock lock(optimization_requested_mutex_); - optimization_requested_.wait(lock, [this]{ return optimization_request_ || !ros::ok(); }); // NOLINT + optimization_requested_.wait(lock, [this] { return optimization_request_ || !ros::ok(); }); // NOLINT } // If a shutdown is requested, exit now. if (!ros::ok()) @@ -180,9 +175,7 @@ void BatchOptimizer::optimizerTimerCallback(const ros::TimerEvent& /*event*/) } } -void BatchOptimizer::transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void BatchOptimizer::transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // Add the new transaction to the pending set // Either we haven't "started" yet and we want to keep a short history of transactions around diff --git a/fuse_optimizers/src/batch_optimizer_node.cpp b/fuse_optimizers/src/batch_optimizer_node.cpp index 1cd84b749..cc1dcaeda 100644 --- a/fuse_optimizers/src/batch_optimizer_node.cpp +++ b/fuse_optimizers/src/batch_optimizer_node.cpp @@ -35,8 +35,7 @@ #include #include - -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "batch_optimizer_node"); fuse_optimizers::BatchOptimizer optimizer(fuse_graphs::HashGraph::make_unique()); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 612f78993..6500aed86 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -50,6 +50,7 @@ namespace fuse_optimizers { +<<<<<<< HEAD FixedLagSmoother::FixedLagSmoother( fuse_core::Graph::UniquePtr graph, @@ -59,6 +60,21 @@ namespace fuse_optimizers params_(params) { } +======= +FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) + : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), params_(params) +{ +} + +FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) + : FixedLagSmoother::FixedLagSmoother( + std::move(graph), ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), + node_handle, private_node_handle) +{ +} +>>>>>>> Address PR comments FixedLagSmoother::FixedLagSmoother( fuse_core::Graph::UniquePtr graph, @@ -76,11 +92,21 @@ namespace fuse_optimizers timestamp_tracking_.addNewTransaction(new_transaction); } +<<<<<<< HEAD std::vector FixedLagSmoother::computeVariablesToMarginalize() { // Find the most recent variable timestamp, then carefully subtract the lag duration. // ROS Time objects do not handle negative values. auto start_time = getStartTime(); +======= + std::lock_guard lock(mutex_); + auto now = timestamp_tracking_[timestamp_tracking_.numStates() - 1]; + lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; + auto marginalize_variable_uuids = std::vector(); + timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); + return marginalize_variable_uuids; +} +>>>>>>> Address PR comments std::lock_guard lock(mutex_); auto now = timestamp_tracking_.currentStamp(); @@ -92,8 +118,17 @@ namespace fuse_optimizers void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction) { +<<<<<<< HEAD std::lock_guard lock(mutex_); timestamp_tracking_.addMarginalTransaction(marginal_transaction); +======= + ROS_DEBUG_STREAM("The current lag expiration time is " + << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() + << " from sensor " << sensor_name << " has a minimum involved timestamp of " << min_stamp + << ", which is " << (lag_expiration_ - min_stamp) + << " seconds too old. Ignoring this transaction."); + return false; +>>>>>>> Address PR comments } bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction) diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 942423d2e..0a51a4645 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -37,8 +37,7 @@ #include #include - -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "fixed_lag_smoother_node"); ros::NodeHandle private_node_handle("~"); diff --git a/fuse_optimizers/src/fixed_size_smoother.cpp b/fuse_optimizers/src/fixed_size_smoother.cpp index 4fea8b83d..545a1ae8f 100644 --- a/fuse_optimizers/src/fixed_size_smoother.cpp +++ b/fuse_optimizers/src/fixed_size_smoother.cpp @@ -50,25 +50,17 @@ namespace fuse_optimizers { -FixedSizeSmoother::FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), - params_(params) +FixedSizeSmoother::FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) + : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), params_(params) { } -FixedSizeSmoother::FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - FixedSizeSmoother::FixedSizeSmoother( - std::move(graph), - ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), - node_handle, - private_node_handle) +FixedSizeSmoother::FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) + : FixedSizeSmoother::FixedSizeSmoother( + std::move(graph), ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), + node_handle, private_node_handle) { } @@ -86,9 +78,10 @@ std::vector FixedSizeSmoother::computeVariablesToMarginalize() // if the total number of states is greater than our optimization window, then find the new state // given we remove the first n states to bring the number of states back to our desired window size - if((int)timestamp_tracking_.numStates() > params_->num_states){ + if ((int)timestamp_tracking_.numStates() > params_->num_states) + { size_t num_states_to_marginalize = timestamp_tracking_.numStates() - params_->num_states; - ros::Time new_start_time = timestamp_tracking_.ithStamp(num_states_to_marginalize - 1); + ros::Time new_start_time = timestamp_tracking_[num_states_to_marginalize - 1]; timestamp_tracking_.query(new_start_time, std::back_inserter(marginalize_variable_uuids)); } @@ -105,14 +98,14 @@ bool FixedSizeSmoother::validateTransaction(const std::string& sensor_name, cons { auto min_stamp = transaction.minStamp(); std::lock_guard lock(mutex_); - ros::Time window_start = timestamp_tracking_.firstStamp(); + ros::Time window_start = timestamp_tracking_[0]; if (min_stamp < window_start) { - ROS_DEBUG_STREAM( - "The current optimization window starts at " - << window_start << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " - << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is prior to the beginning " - << "of this window, ignoring this transaction."); + ROS_DEBUG_STREAM("The current optimization window starts at " + << window_start << ". The queued transaction with timestamp " << transaction.stamp() + << " from sensor " << sensor_name << " has a minimum involved timestamp of " << min_stamp + << ", which is prior to the beginning " + << "of this window, ignoring this transaction."); return false; } return true; diff --git a/fuse_optimizers/src/fixed_size_smoother_node.cpp b/fuse_optimizers/src/fixed_size_smoother_node.cpp index 2e7c65ce3..d6360e2cb 100644 --- a/fuse_optimizers/src/fixed_size_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_size_smoother_node.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ #include #include - -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "fixed_size_smoother_node"); ros::NodeHandle private_node_handle("~"); diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 203ee1769..97ebfaa4a 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -49,10 +49,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief Plugin name and type configuration, as received from the parameter server * @@ -79,17 +77,15 @@ struct PluginConfig XmlRpc::XmlRpcValue config; //!< The entire configuration, that might have additional optional parameters }; -Optimizer::Optimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - graph_(std::move(graph)), - node_handle_(node_handle), - private_node_handle_(private_node_handle), - motion_model_loader_("fuse_core", "fuse_core::MotionModel"), - publisher_loader_("fuse_core", "fuse_core::Publisher"), - sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), - diagnostic_updater_(node_handle_) +Optimizer::Optimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) + : graph_(std::move(graph)) + , node_handle_(node_handle) + , private_node_handle_(private_node_handle) + , motion_model_loader_("fuse_core", "fuse_core::MotionModel") + , publisher_loader_("fuse_core", "fuse_core::Publisher") + , sensor_model_loader_("fuse_core", "fuse_core::SensorModel") + , diagnostic_updater_(node_handle_) { // Setup diagnostics updater private_node_handle_.param("diagnostic_updater_timer_period", diagnostic_updater_timer_period_, @@ -138,12 +134,12 @@ void Optimizer::loadMotionModels() { // Validate the parameter server values const auto& motion_model = motion_models[motion_model_index]; - if ( (motion_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!motion_model.hasMember("name")) - || (!motion_model.hasMember("type"))) + if ((motion_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!motion_model.hasMember("name")) || + (!motion_model.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " - "-{name: string, type: string}"); + throw std::invalid_argument( + "The 'motion_models' parameter should be a list of the form: " + "-{name: string, type: string}"); } motion_model_configs.emplace_back(static_cast(motion_model["name"]), @@ -156,11 +152,12 @@ void Optimizer::loadMotionModels() for (const auto& motion_model : motion_models) { const auto& motion_model_config = motion_model.second; - if ( (motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!motion_model_config.hasMember("type"))) + if ((motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || + (!motion_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a struct of the form: " - "{string: {type: string}}"); + throw std::invalid_argument( + "The 'motion_models' parameter should be a struct of the form: " + "{string: {type: string}}"); } motion_model_configs.emplace_back(static_cast(motion_model.first), @@ -169,8 +166,9 @@ void Optimizer::loadMotionModels() } else { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " - "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); + throw std::invalid_argument( + "The 'motion_models' parameter should be a list of the form: " + "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } for (const auto& config : motion_model_configs) @@ -204,12 +202,12 @@ void Optimizer::loadSensorModels() { // Validate the parameter server values const auto& sensor_model = sensor_models[sensor_model_index]; - if ( (sensor_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!sensor_model.hasMember("name")) - || (!sensor_model.hasMember("type"))) + if ((sensor_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!sensor_model.hasMember("name")) || + (!sensor_model.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " - "-{name: string, type: string, motion_models: [name1, name2, ...]}"); + throw std::invalid_argument( + "The 'sensor_models' parameter should be a list of the form: " + "-{name: string, type: string, motion_models: [name1, name2, ...]}"); } sensor_model_configs.emplace_back(static_cast(sensor_model["name"]), @@ -223,11 +221,12 @@ void Optimizer::loadSensorModels() { // Validate the parameter server values const auto& sensor_model_config = sensor_model.second; - if ( (sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!sensor_model_config.hasMember("type"))) + if ((sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || + (!sensor_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a struct of the form: " - "{string: {type: string, motion_models: [name1, name2, ...]}}"); + throw std::invalid_argument( + "The 'sensor_models' parameter should be a struct of the form: " + "{string: {type: string, motion_models: [name1, name2, ...]}}"); } sensor_model_configs.emplace_back(static_cast(sensor_model.first), @@ -236,10 +235,11 @@ void Optimizer::loadSensorModels() } else { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " - "-{name: string, type: string, motion_models: [name1, name2, ...]} " - "or a struct of the form: " - "{string: {type: string, motion_models: [name1, name2, ...]}}"); + throw std::invalid_argument( + "The 'sensor_models' parameter should be a list of the form: " + "-{name: string, type: string, motion_models: [name1, name2, ...]} " + "or a struct of the form: " + "{string: {type: string, motion_models: [name1, name2, ...]}}"); } for (const auto& config : sensor_model_configs) @@ -250,16 +250,15 @@ void Optimizer::loadSensorModels() // Create a sensor object using pluginlib. This will throw if the plugin name is not found. auto sensor_model = sensor_model_loader_.createUniqueInstance(config.type); // Initialize the sensor - sensor_model->initialize( - config.name, - std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); + sensor_model->initialize(config.name, + std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); // Store the sensor in a member variable for use later sensor_models_.emplace(config.name, SensorModelInfo{ std::move(sensor_model), ignition }); // NOLINT(whitespace/braces) // Parse out the list of associated motion models, if any - if ( (config.config.hasMember("motion_models")) - && (config.config["motion_models"].getType() == XmlRpc::XmlRpcValue::TypeArray)) + if ((config.config.hasMember("motion_models")) && + (config.config["motion_models"].getType() == XmlRpc::XmlRpcValue::TypeArray)) { XmlRpc::XmlRpcValue motion_model_list = config.config["motion_models"]; for (int32_t motion_model_index = 0; motion_model_index < motion_model_list.size(); ++motion_model_index) @@ -268,9 +267,10 @@ void Optimizer::loadSensorModels() associated_motion_models_[config.name].push_back(motion_model_name); if (motion_models_.find(motion_model_name) == motion_models_.end()) { - ROS_WARN_STREAM("Sensor model '" << config.name << "' is configured to use motion model '" << - motion_model_name << "', but no motion model with that name currently exists. This is " << - "likely a configuration error."); + ROS_WARN_STREAM("Sensor model '" << config.name << "' is configured to use motion model '" + << motion_model_name + << "', but no motion model with that name currently exists. This is " + << "likely a configuration error."); } } } @@ -297,12 +297,12 @@ void Optimizer::loadPublishers() { // Validate the parameter server values const auto& publisher = publishers[publisher_index]; - if ( (publisher.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!publisher.hasMember("name")) - || (!publisher.hasMember("type"))) + if ((publisher.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher.hasMember("name")) || + (!publisher.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string}"); + throw std::invalid_argument( + "The 'publishers' parameter should be a list of the form: " + "-{name: string, type: string}"); } publisher_configs.emplace_back(static_cast(publisher["name"]), @@ -316,11 +316,11 @@ void Optimizer::loadPublishers() { // Validate the parameter server values const auto& publisher_config = publisher.second; - if ( (publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!publisher_config.hasMember("type"))) + if ((publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher_config.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a struct of the form: " - "{string: {type: string}}"); + throw std::invalid_argument( + "The 'publishers' parameter should be a struct of the form: " + "{string: {type: string}}"); } publisher_configs.emplace_back(static_cast(publisher.first), @@ -329,8 +329,9 @@ void Optimizer::loadPublishers() } else { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); + throw std::invalid_argument( + "The 'publishers' parameter should be a list of the form: " + "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } for (const auto& config : publisher_configs) @@ -346,9 +347,7 @@ void Optimizer::loadPublishers() diagnostic_updater_.force_update(); } -bool Optimizer::applyMotionModels( - const std::string& sensor_name, - fuse_core::Transaction& transaction) const +bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const { // Check for trivial cases where we don't have to do anything auto iter = associated_motion_models_.find(sensor_name); @@ -368,16 +367,15 @@ bool Optimizer::applyMotionModels( catch (const std::exception& e) { ROS_ERROR_STREAM("Error generating constraints for sensor '" << sensor_name << "' " - << "from motion model '" << motion_model_name << "'. Error: " << e.what()); + << "from motion model '" << motion_model_name + << "'. Error: " << e.what()); success = false; } } return success; } -void Optimizer::notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph) { for (const auto& name__sensor_model : sensor_models_) { @@ -387,8 +385,8 @@ void Optimizer::notify( } catch (const std::exception& e) { - ROS_ERROR_STREAM("Failed calling graphCallback() on sensor '" << name__sensor_model.first << "'. " << - "Error: " << e.what()); + ROS_ERROR_STREAM("Failed calling graphCallback() on sensor '" << name__sensor_model.first << "'. " + << "Error: " << e.what()); continue; } } @@ -400,8 +398,8 @@ void Optimizer::notify( } catch (const std::exception& e) { - ROS_ERROR_STREAM("Failed calling graphCallback() on motion model '" << name__motion_model.first << "." << - " Error: " << e.what()); + ROS_ERROR_STREAM("Failed calling graphCallback() on motion model '" << name__motion_model.first << "." + << " Error: " << e.what()); continue; } } @@ -413,24 +411,22 @@ void Optimizer::notify( } catch (const std::exception& e) { - ROS_ERROR_STREAM("Failed calling notify() on publisher '" << name__publisher.first << "." << - " Error: " << e.what()); + ROS_ERROR_STREAM("Failed calling notify() on publisher '" << name__publisher.first << "." + << " Error: " << e.what()); continue; } } } -void Optimizer::injectCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void Optimizer::injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // We are going to insert a call to the derived class's transactionCallback() method into the global callback queue. // This returns execution to the sensor's thread quickly by moving the transaction processing to the optimizer's // thread. And by using the existing ROS callback queue, we simplify the threading model of the optimizer. ros::getGlobalCallbackQueue()->addCallback( - boost::make_shared>( - std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))), - reinterpret_cast(this)); + boost::make_shared>( + std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))), + reinterpret_cast(this)); } void Optimizer::clearCallbacks() diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index d8b63bded..65312cf58 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -45,31 +45,23 @@ namespace fuse_optimizers { -ros::Time VariableStampIndex::currentStamp() const +ros::Time VariableStampIndex::operator[](int i) { - if(!unique_stamps_.empty()){ - return *unique_stamps_.end(); + if (unique_stamps_.empty() || i < 0) + { + return ros::Time(0, 0); } - return ros::Time(0, 0); -} - -ros::Time VariableStampIndex::firstStamp() const -{ - if(!unique_stamps_.empty()){ - return *unique_stamps_.begin(); + else if ((size_t)i < unique_stamps_.size()) + { + uint64_t stamp_id = (*std::next(unique_stamps_.begin(), i)).first; + ros::Time stamp; + stamp.fromNSec(stamp_id); + return stamp; } - return ros::Time(0, 0); -} - -ros::Time VariableStampIndex::ithStamp(size_t idx) const{ - if(idx < unique_stamps_.size()){ - std::set::iterator it = unique_stamps_.begin(); - for(size_t i = 0; i < idx; i++){ - it++; - } - return *(it); + else + { + throw std::runtime_error("Index exceeds size of Variable Stamp Index."); } - return ros::Time(0, 0); } void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transaction) @@ -108,10 +100,16 @@ void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& trans auto stamped_variable = dynamic_cast(&variable); if (stamped_variable) { + uint64_t stamp = stamped_variable->stamp().toNSec(); stamped_index_[variable.uuid()] = stamped_variable->stamp(); - // add to unique stamps - if(unique_stamps_.find(stamped_variable->stamp()) == unique_stamps_.end()){ - unique_stamps_.insert(stamped_variable->stamp()); + if (unique_stamps_.find(stamp) != unique_stamps_.end()) + { + unique_stamps_[stamp].insert(variable.uuid()); + } + else + { + std::unordered_set set = { variable.uuid() }; + unique_stamps_.insert({ stamp, set }); } } variables_[variable.uuid()]; // Add an empty set of constraints @@ -135,9 +133,17 @@ void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction& tra for (const auto& variable_uuid : transaction.removedVariables()) { // remove from unique stamps - auto stamp = stamped_index_[variable_uuid]; - if(unique_stamps_.find(stamp) != unique_stamps_.end()){ - unique_stamps_.erase(stamp); + auto stamp = stamped_index_[variable_uuid].toNSec(); + if (unique_stamps_.find(stamp) != unique_stamps_.end()) + { + if (unique_stamps_[stamp].find(variable_uuid) != unique_stamps_[stamp].end()) + { + unique_stamps_[stamp].erase(variable_uuid); + } + if (unique_stamps_[stamp].size() == 0) + { + unique_stamps_.erase(stamp); + } } stamped_index_.erase(variable_uuid); diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp index b62c41932..184b0b745 100644 --- a/fuse_optimizers/src/windowed_optimizer.cpp +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -58,9 +58,8 @@ namespace * @return A reverse iterator pointing to the element after the erased element */ template -typename std::vector::reverse_iterator erase( - std::vector& container, - typename std::vector::reverse_iterator position) +typename std::vector::reverse_iterator erase(std::vector& container, + typename std::vector::reverse_iterator position) { // Reverse iterators are weird // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator @@ -76,17 +75,14 @@ typename std::vector::reverse_iterator erase( namespace fuse_optimizers { -WindowedOptimizer::WindowedOptimizer( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), - params_(params), - ignited_(false), - optimization_running_(true), - started_(false), - optimization_request_(false) +WindowedOptimizer::WindowedOptimizer(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) + : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) + , params_(params) + , ignited_(false) + , optimization_running_(true) + , started_(false) + , optimization_request_(false) { // Test for auto-start autostart(); @@ -96,19 +92,15 @@ WindowedOptimizer::WindowedOptimizer( // Configure a timer to trigger optimizations optimize_timer_ = - node_handle_.createTimer(params_->optimization_period, &WindowedOptimizer::optimizerTimerCallback, this); + node_handle_.createTimer(params_->optimization_period, &WindowedOptimizer::optimizerTimerCallback, this); // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = node_handle_.advertiseService( - ros::names::resolve(params_->reset_service), - &WindowedOptimizer::resetServiceCallback, - this); + reset_service_server_ = node_handle_.advertiseService(ros::names::resolve(params_->reset_service), + &WindowedOptimizer::resetServiceCallback, this); // Subscribe to a reset message that will reset the optimizer to the initial state - reset_subscriber_ = node_handle_.subscribe( - ros::names::resolve(params_->reset_service), 10, - &WindowedOptimizer::resetMessageCallback, - this); + reset_subscriber_ = node_handle_.subscribe(ros::names::resolve(params_->reset_service), 10, + &WindowedOptimizer::resetMessageCallback, this); } WindowedOptimizer::~WindowedOptimizer() @@ -125,10 +117,8 @@ WindowedOptimizer::~WindowedOptimizer() void WindowedOptimizer::autostart() { - if (std::none_of( - sensor_models_.begin(), - sensor_models_.end(), - [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) + if (std::none_of(sensor_models_.begin(), sensor_models_.end(), + [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) { // No ignition sensors were provided. Auto-start. started_ = true; @@ -139,8 +129,7 @@ void WindowedOptimizer::autostart() void WindowedOptimizer::optimizationLoop() { - auto exit_wait_condition = [this]() - { + auto exit_wait_condition = [this]() { return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); }; // Optimize constraints until told to exit @@ -177,19 +166,25 @@ void WindowedOptimizer::optimizationLoop() } // check if new transaction has added constraints to variables that are to be marginalized std::vector faulty_constraints; - for(auto& c: new_transaction->addedConstraints()){ - for(auto var_uuid: c.variables()){ - for (auto marginal_uuid : marginal_transaction_.removedVariables()) { - if (var_uuid == marginal_uuid) { + for (auto& c : new_transaction->addedConstraints()) + { + for (auto var_uuid : c.variables()) + { + for (auto marginal_uuid : marginal_transaction_.removedVariables()) + { + if (var_uuid == marginal_uuid) + { faulty_constraints.push_back(c.uuid()); break; } } } } - if(faulty_constraints.size() > 0){ + if (faulty_constraints.size() > 0) + { ROS_WARN_STREAM("Removing invalid constraints."); - for(auto& faulty_constraint: faulty_constraints){ + for (auto& faulty_constraint : faulty_constraints) + { new_transaction->removeConstraint(faulty_constraint); } } @@ -211,11 +206,10 @@ void WindowedOptimizer::optimizationLoop() oss << "\nTransaction:\n"; new_transaction->print(oss); - ROS_FATAL_STREAM( - "Failed to update graph with transaction: " << ex.what() - << "\nLeaving optimization loop and requesting " - "node shutdown...\n" - << oss.str()); + ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() + << "\nLeaving optimization loop and requesting " + "node shutdown...\n" + << oss.str()); ros::requestShutdown(); break; } @@ -229,9 +223,8 @@ void WindowedOptimizer::optimizationLoop() // Abort if optimization failed. Not converging is not a failure because the solution found is usable. if (!summary_.IsSolutionUsable()) { - ROS_FATAL_STREAM( - "Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); ROS_INFO_STREAM(summary_.FullReport()); ros::requestShutdown(); break; @@ -241,7 +234,7 @@ void WindowedOptimizer::optimizationLoop() // Determination of which variables to marginalize is delegated to derived classes auto variables_to_marginalize = computeVariablesToMarginalize(); marginal_transaction_ = - fuse_constraints::marginalizeVariables(ros::this_node::getName(), variables_to_marginalize, *graph_); + fuse_constraints::marginalizeVariables(ros::this_node::getName(), variables_to_marginalize, *graph_); // Perform any post-marginal cleanup -- Delegated to derived classes postprocessMarginalization(marginal_transaction_); // Note: The marginal transaction will not be applied until the next optimization iteration @@ -249,10 +242,8 @@ void WindowedOptimizer::optimizationLoop() auto optimization_complete = ros::Time::now(); if (optimization_complete > optimization_deadline) { - ROS_WARN_STREAM_THROTTLE( - 10.0, - "Optimization exceeded the configured duration by " << (optimization_complete - optimization_deadline) - << "s"); + ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline) << "s"); } } } @@ -315,10 +306,10 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) { // We just started, but the oldest transaction is not from an ignition sensor. We will still process the // transaction, but we do not enforce it is processed individually. - ROS_ERROR_STREAM( - "The queued transaction with timestamp " << element.stamp() << " from sensor " << element.sensor_name - << " is not an ignition sensor transaction. " - << "This transaction will not be processed individually."); + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.stamp() << " from sensor " << element.sensor_name + << " is not an ignition sensor transaction. " + << "This transaction will not be processed individually."); } else { @@ -333,21 +324,20 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) { // The motion model processing failed. When this happens to an ignition sensor transaction there is no point on // trying again next time, so we ignore this transaction. - ROS_ERROR_STREAM( - "The queued ignition transaction with timestamp " << element.stamp() << " from sensor " << element.sensor_name - << " could not be processed. Ignoring this ignition " - "transaction."); + ROS_ERROR_STREAM("The queued ignition transaction with timestamp " + << element.stamp() << " from sensor " << element.sensor_name + << " could not be processed. Ignoring this ignition " + "transaction."); // Remove the ignition transaction that just failed and purge all transactions after it. But if we find another // ignition transaction, we schedule it to be processed in the next optimization cycle. erase(pending_transactions_, transaction_rbegin); - const auto pending_ignition_transaction_iter = std::find_if( - pending_transactions_.rbegin(), - pending_transactions_.rend(), - [this](const auto& element) { // NOLINT(whitespace/braces) - return sensor_models_.at(element.sensor_name).ignition; - }); // NOLINT(whitespace/braces) + const auto pending_ignition_transaction_iter = + std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), + [this](const auto& element) { // NOLINT(whitespace/braces) + return sensor_models_.at(element.sensor_name).ignition; + }); // NOLINT(whitespace/braces) if (pending_ignition_transaction_iter == pending_transactions_.rend()) { // There is no other ignition transaction pending. We simply roll back to not started state and all other @@ -401,15 +391,13 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) if (max_stamp + params_->transaction_timeout < current_time) { // Warn that this transaction has expired, then skip it. - ROS_ERROR_STREAM( - "The queued transaction with timestamp " << element.stamp() - << " and maximum " - "involved stamp of " - << max_stamp << " from sensor " << element.sensor_name - << " could not be processed after " << (current_time - max_stamp) - << " seconds, " - "which is greater than the 'transaction_timeout' value of " - << params_->transaction_timeout << ". Ignoring this transaction."); + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.stamp() << " and maximum " + "involved stamp of " + << max_stamp << " from sensor " << element.sensor_name << " could not be processed after " + << (current_time - max_stamp) << " seconds, " + "which is greater than the 'transaction_timeout' value of " + << params_->transaction_timeout << ". Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); } else @@ -422,7 +410,8 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) } } -void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&){ +void WindowedOptimizer::reset() +{ // Tell all the plugins to stop stopPlugins(); // Reset the optimizer state @@ -456,56 +445,32 @@ void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&){ return; } -bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&) { - // Tell all the plugins to stop - stopPlugins(); - // Reset the optimizer state - { - std::lock_guard lock(optimization_requested_mutex_); - optimization_request_ = false; - } - started_ = false; - ignited_ = false; - setStartTime(ros::Time(0, 0)); - // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the - // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to - // prevent the possibility of deadlocks. - { - std::lock_guard lock(optimization_mutex_); - // Clear all pending transactions - { - std::lock_guard lock(pending_transactions_mutex_); - pending_transactions_.clear(); - } - // Clear the graph and marginal tracking states - graph_->clear(); - marginal_transaction_ = fuse_core::Transaction(); - } - // Perform any required reset operations for derived classes - onReset(); - // Tell all the plugins to start - startPlugins(); - // Test for auto-start - autostart(); + // perform reset logic + reset(); + return; +} +bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + // perform reset logic + reset(); return true; } -void WindowedOptimizer::transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void WindowedOptimizer::transactionCallback(const std::string& sensor_name, + fuse_core::Transaction::SharedPtr transaction) { // If this transaction occurs before the start time, just ignore it auto start_time = getStartTime(); const auto max_time = transaction->maxStamp(); if (started_ && max_time < start_time) { - ROS_DEBUG_STREAM( - "Received a transaction before the start time from sensor '" - << sensor_name << "'.\n" - << " start_time: " << start_time << ", maximum involved stamp: " << max_time - << ", difference: " << (start_time - max_time) << "s"); + ROS_DEBUG_STREAM("Received a transaction before the start time from sensor '" + << sensor_name << "'.\n" + << " start_time: " << start_time << ", maximum involved stamp: " << max_time + << ", difference: " << (start_time - max_time) << "s"); return; } { @@ -514,12 +479,11 @@ void WindowedOptimizer::transactionCallback( // Add the new transaction to the pending set // The pending set is arranged "smallest stamp last" to making popping off the back more efficient - auto comparator = [](const ros::Time& value, const TransactionQueueElement& element) - { + auto comparator = [](const ros::Time& value, const TransactionQueueElement& element) { return value >= element.stamp(); }; auto position = - std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); + std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT // If we haven't "started" yet.. @@ -540,16 +504,13 @@ void WindowedOptimizer::transactionCallback( // TODO(efernandez) Do '&min_time = std::as_const(start_time)' when C++17 is supported and we can use // std::as_const: https://en.cppreference.com/w/cpp/utility/as_const pending_transactions_.erase( - std::remove_if( - pending_transactions_.begin(), - pending_transactions_.end(), - [&sensor_name, - max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); + std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), + [&sensor_name, max_time, + &min_time = start_time ](const auto& transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); } else { diff --git a/fuse_optimizers/test/example_optimizer.h b/fuse_optimizers/test/example_optimizer.h index 1464b2615..7708f5720 100644 --- a/fuse_optimizers/test/example_optimizer.h +++ b/fuse_optimizers/test/example_optimizer.h @@ -39,7 +39,6 @@ #include #include - /** * @brief Example optimizer that exposes the motion and sensor models, and the publishers, so we can check the expected * ones are loaded. @@ -50,7 +49,7 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer SMART_PTR_DEFINITIONS(ExampleOptimizer); ExampleOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")) + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")) : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) { } @@ -70,9 +69,7 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer return publishers_; } - void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) override + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override { } }; diff --git a/fuse_optimizers/test/test_fixed_lag_ignition.cpp b/fuse_optimizers/test/test_fixed_lag_ignition.cpp index a18571028..705d64a54 100644 --- a/fuse_optimizers/test/test_fixed_lag_ignition.cpp +++ b/fuse_optimizers/test/test_fixed_lag_ignition.cpp @@ -38,7 +38,6 @@ #include - TEST(FixedLagIgnition, SetInitialState) { // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify. @@ -72,8 +71,7 @@ TEST(FixedLagIgnition, SetInitialState) // The 'set_pose' service call triggers all of the sensors to resubscribe to their topics. // I need to wait for those subscribers to be ready before sending them sensor data. ros::WallTime subscriber_timeout = ros::WallTime::now() + ros::WallDuration(1.0); - while ((relative_pose_publisher.getNumSubscribers() < 1u) && - (ros::WallTime::now() < subscriber_timeout)) + while ((relative_pose_publisher.getNumSubscribers() < 1u) && (ros::WallTime::now() < subscriber_timeout)) { ros::WallDuration(0.01).sleep(); } @@ -113,8 +111,7 @@ TEST(FixedLagIgnition, SetInitialState) // Wait for the optimizer to process all queued transactions ros::Time result_timeout = ros::Time::now() + ros::Duration(3.0); auto odom_msg = nav_msgs::Odometry::ConstPtr(); - while ((!odom_msg || odom_msg->header.stamp != ros::Time(3, 0)) && - (ros::Time::now() < result_timeout)) + while ((!odom_msg || odom_msg->header.stamp != ros::Time(3, 0)) && (ros::Time::now() < result_timeout)) { odom_msg = ros::topic::waitForMessage("/odom", ros::Duration(1.0)); } diff --git a/fuse_optimizers/test/test_optimizer.cpp b/fuse_optimizers/test/test_optimizer.cpp index 804e82bba..f1d075de6 100644 --- a/fuse_optimizers/test/test_optimizer.cpp +++ b/fuse_optimizers/test/test_optimizer.cpp @@ -42,7 +42,6 @@ #include #include - TEST(Optimizer, Constructor) { // Create optimizer: @@ -63,12 +62,12 @@ TEST(Optimizer, Constructor) "unicycle_2d_ignition", "wheel_odometry" }; const std::vector expected_publishers = { "odometry_publisher", "serialized_publisher" }; - ASSERT_TRUE(std::is_sorted(expected_motion_models.begin(), expected_motion_models.end())) - << expected_motion_models << " is not sorted."; - ASSERT_TRUE(std::is_sorted(expected_sensor_models.begin(), expected_sensor_models.end())) - << expected_sensor_models << " is not sorted."; - ASSERT_TRUE(std::is_sorted(expected_publishers.begin(), expected_publishers.end())) - << expected_publishers << " is not sorted."; + ASSERT_TRUE(std::is_sorted(expected_motion_models.begin(), expected_motion_models.end())) << expected_motion_models + << " is not sorted."; + ASSERT_TRUE(std::is_sorted(expected_sensor_models.begin(), expected_sensor_models.end())) << expected_sensor_models + << " is not sorted."; + ASSERT_TRUE(std::is_sorted(expected_publishers.begin(), expected_publishers.end())) << expected_publishers + << " is not sorted."; // Compute the symmetric difference between the expected and actual motion and sensor models, and publishers: const auto difference_motion_models = set_symmetric_difference(expected_motion_models, motion_models); @@ -77,14 +76,14 @@ TEST(Optimizer, Constructor) // Check the symmetric difference is empty, i.e. the actual motion and sensor models, and publishers are the same as // the expected ones: - EXPECT_TRUE(difference_motion_models.empty()) - << "Actual: " << motion_models << "\nExpected: " << expected_motion_models - << "\nDifference: " << difference_motion_models; - EXPECT_TRUE(difference_sensor_models.empty()) - << "Actual: " << sensor_models << "\nExpected: " << expected_sensor_models - << "\nDifference: " << difference_sensor_models; - EXPECT_TRUE(difference_publishers.empty()) - << "Actual: " << publishers << "\nExpected: " << expected_publishers << "\nDifference: " << difference_publishers; + EXPECT_TRUE(difference_motion_models.empty()) << "Actual: " << motion_models + << "\nExpected: " << expected_motion_models + << "\nDifference: " << difference_motion_models; + EXPECT_TRUE(difference_sensor_models.empty()) << "Actual: " << sensor_models + << "\nExpected: " << expected_sensor_models + << "\nDifference: " << difference_sensor_models; + EXPECT_TRUE(difference_publishers.empty()) << "Actual: " << publishers << "\nExpected: " << expected_publishers + << "\nDifference: " << difference_publishers; } int main(int argc, char** argv) diff --git a/fuse_optimizers/test/test_variable_stamp_index.cpp b/fuse_optimizers/test/test_variable_stamp_index.cpp index 2fd9e8be3..4e2e739ff 100644 --- a/fuse_optimizers/test/test_variable_stamp_index.cpp +++ b/fuse_optimizers/test/test_variable_stamp_index.cpp @@ -51,7 +51,6 @@ #include #include - /** * @brief Create a simple stamped Variable for testing */ @@ -60,10 +59,8 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp public: FUSE_VARIABLE_DEFINITIONS(StampedVariable); - explicit StampedVariable(const ros::Time& stamp = ros::Time(0, 0)) : - fuse_core::Variable(fuse_core::uuid::generate()), - fuse_variables::Stamped(stamp), - data_{} + explicit StampedVariable(const ros::Time& stamp = ros::Time(0, 0)) + : fuse_core::Variable(fuse_core::uuid::generate()), fuse_variables::Stamped(stamp), data_{} { } @@ -98,12 +95,12 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template + template void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -117,9 +114,7 @@ class UnstampedVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(UnstampedVariable); - UnstampedVariable() : - fuse_core::Variable(fuse_core::uuid::generate()), - data_{} + UnstampedVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_{} { } @@ -154,11 +149,11 @@ class UnstampedVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template + template void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -174,30 +169,24 @@ class GenericConstraint : public fuse_core::Constraint GenericConstraint() = default; - GenericConstraint(const std::string& source, std::initializer_list variable_uuids) : - Constraint(source, variable_uuids) + GenericConstraint(const std::string& source, std::initializer_list variable_uuids) + : Constraint(source, variable_uuids) { } - explicit GenericConstraint(const std::string& source, const fuse_core::UUID& variable1) : - fuse_core::Constraint(source, {variable1}) + explicit GenericConstraint(const std::string& source, const fuse_core::UUID& variable1) + : fuse_core::Constraint(source, { variable1 }) { } - GenericConstraint( - const std::string& source, - const fuse_core::UUID& variable1, - const fuse_core::UUID& variable2) : - fuse_core::Constraint(source, {variable1, variable2}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + : fuse_core::Constraint(source, { variable1, variable2 }) { } - GenericConstraint( - const std::string& source, - const fuse_core::UUID& variable1, - const fuse_core::UUID& variable2, - const fuse_core::UUID& variable3) : - fuse_core::Constraint(source, {variable1, variable2, variable3}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2, + const fuse_core::UUID& variable3) + : fuse_core::Constraint(source, { variable1, variable2, variable3 }) { } @@ -220,16 +209,15 @@ class GenericConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template + template void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; BOOST_CLASS_EXPORT(GenericConstraint); - TEST(VariableStampIndex, Size) { // Create an empty index @@ -261,7 +249,7 @@ TEST(VariableStampIndex, CurrentStamp) auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -270,7 +258,7 @@ TEST(VariableStampIndex, CurrentStamp) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add a stamped variable auto x2 = StampedVariable::make_shared(ros::Time(1, 0)); @@ -279,7 +267,7 @@ TEST(VariableStampIndex, CurrentStamp) index.addNewTransaction(transaction2); // Verify the current stamp is now Time(1, 0) - EXPECT_EQ(ros::Time(1, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(1, 0), index[index.numStates() - 1]); } TEST(VariableStampIndex, FirstStamp) @@ -288,7 +276,7 @@ TEST(VariableStampIndex, FirstStamp) auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -297,7 +285,7 @@ TEST(VariableStampIndex, FirstStamp) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add two stamped variables auto x2 = StampedVariable::make_shared(ros::Time(1, 0)); @@ -311,7 +299,7 @@ TEST(VariableStampIndex, FirstStamp) index.addNewTransaction(transaction3); // Verify the current stamp is now Time(1, 0) - EXPECT_EQ(ros::Time(1, 0), index.firstStamp()); + EXPECT_EQ(ros::Time(1, 0), index[0]); } TEST(VariableStampIndex, NumSates) @@ -320,7 +308,7 @@ TEST(VariableStampIndex, NumSates) auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -329,12 +317,13 @@ TEST(VariableStampIndex, NumSates) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add N stamped variables int N = 10; - for(int i = 0; i < N; i++){ + for (int i = 0; i < N; i++) + { auto x = StampedVariable::make_shared(ros::Time(i, 0)); auto transaction = fuse_core::Transaction(); transaction.addVariable(x); @@ -342,17 +331,16 @@ TEST(VariableStampIndex, NumSates) } // Verify the number of unique stamps - EXPECT_EQ((size_t)N, index.numStates()); + EXPECT_EQ(N, index.numStates()); } - TEST(VariableStampIndex, ithStamp) { // Create an empty index auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -361,11 +349,12 @@ TEST(VariableStampIndex, ithStamp) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add N stamped variables int N = 10; - for(int i = 0; i < N; i++){ + for (int i = 0; i < N; i++) + { auto x = StampedVariable::make_shared(ros::Time(i, 0)); auto transaction = fuse_core::Transaction(); transaction.addVariable(x); @@ -373,8 +362,9 @@ TEST(VariableStampIndex, ithStamp) } // Verify the stamps of the states using ithStamp function - for(int i = 0; i < N; i++){ - EXPECT_EQ(ros::Time(i, 0), index.ithStamp((size_t)i)); + for (int i = 0; i < N; i++) + { + EXPECT_EQ(ros::Time(i, 0), index[i]); } } @@ -414,7 +404,7 @@ TEST(VariableStampIndex, Query) index.query(ros::Time(1, 500000), std::back_inserter(actual1)); EXPECT_EQ(expected1, actual1); - auto expected2 = std::vector{x1->uuid(), l1->uuid()}; + auto expected2 = std::vector{ x1->uuid(), l1->uuid() }; std::sort(expected2.begin(), expected2.end()); auto actual2 = std::vector(); index.query(ros::Time(2, 500000), std::back_inserter(actual2)); @@ -467,7 +457,7 @@ TEST(VariableStampIndex, MarginalTransaction) EXPECT_EQ(4u, index.size()); // And the marginal constraint x3->l1 should not affect future queries - auto expected = std::vector{l1->uuid()}; + auto expected = std::vector{ l1->uuid() }; std::sort(expected.begin(), expected.end()); auto actual = std::vector(); index.query(ros::Time(2, 500000), std::back_inserter(actual)); @@ -475,7 +465,7 @@ TEST(VariableStampIndex, MarginalTransaction) EXPECT_EQ(expected, actual); } -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From 70c03e58559ccff01ad4cf07ab1c7afe83131226 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 18 Jan 2023 16:27:01 -0500 Subject: [PATCH 15/17] formatting --- fuse_optimizers/src/fixed_lag_smoother.cpp | 115 ++++++++------------- 1 file changed, 45 insertions(+), 70 deletions(-) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 6500aed86..f6584970a 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -50,31 +50,19 @@ namespace fuse_optimizers { -<<<<<<< HEAD - - FixedLagSmoother::FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr ¶ms, - const ros::NodeHandle &node_handle, - const ros::NodeHandle &private_node_handle) : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), - params_(params) + FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr ¶ms, + const ros::NodeHandle &node_handle, const ros::NodeHandle &private_node_handle) + : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), params_(params) { } -======= -FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) - : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), params_(params) -{ -} -FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) - : FixedLagSmoother::FixedLagSmoother( - std::move(graph), ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), - node_handle, private_node_handle) -{ -} ->>>>>>> Address PR comments + FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle &node_handle, + const ros::NodeHandle &private_node_handle) + : FixedLagSmoother::FixedLagSmoother( + std::move(graph), ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), + node_handle, private_node_handle) + { + } FixedLagSmoother::FixedLagSmoother( fuse_core::Graph::UniquePtr graph, @@ -92,13 +80,6 @@ FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros: timestamp_tracking_.addNewTransaction(new_transaction); } -<<<<<<< HEAD - std::vector FixedLagSmoother::computeVariablesToMarginalize() - { - // Find the most recent variable timestamp, then carefully subtract the lag duration. - // ROS Time objects do not handle negative values. - auto start_time = getStartTime(); -======= std::lock_guard lock(mutex_); auto now = timestamp_tracking_[timestamp_tracking_.numStates() - 1]; lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; @@ -106,52 +87,46 @@ FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros: timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); return marginalize_variable_uuids; } ->>>>>>> Address PR comments - std::lock_guard lock(mutex_); - auto now = timestamp_tracking_.currentStamp(); - lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; - auto marginalize_variable_uuids = std::vector(); - timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); - return marginalize_variable_uuids; - } +std::lock_guard lock(mutex_); +auto now = timestamp_tracking_.currentStamp(); +lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; +auto marginalize_variable_uuids = std::vector(); +timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); +return marginalize_variable_uuids; +} - void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction) - { -<<<<<<< HEAD - std::lock_guard lock(mutex_); - timestamp_tracking_.addMarginalTransaction(marginal_transaction); -======= - ROS_DEBUG_STREAM("The current lag expiration time is " - << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() - << " from sensor " << sensor_name << " has a minimum involved timestamp of " << min_stamp - << ", which is " << (lag_expiration_ - min_stamp) - << " seconds too old. Ignoring this transaction."); - return false; ->>>>>>> Address PR comments - } +void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction) +{ + ROS_DEBUG_STREAM("The current lag expiration time is " + << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() + << " from sensor " << sensor_name << " has a minimum involved timestamp of " << min_stamp + << ", which is " << (lag_expiration_ - min_stamp) + << " seconds too old. Ignoring this transaction."); + return false; +} - bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction) +bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction) +{ + auto min_stamp = transaction.minStamp(); + std::lock_guard lock(mutex_); + if (min_stamp < lag_expiration_) { - auto min_stamp = transaction.minStamp(); - std::lock_guard lock(mutex_); - if (min_stamp < lag_expiration_) - { - ROS_DEBUG_STREAM( - "The current lag expiration time is " - << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " - << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is " - << (lag_expiration_ - min_stamp) << " seconds too old. Ignoring this transaction."); - return false; - } - return true; + ROS_DEBUG_STREAM( + "The current lag expiration time is " + << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " + << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is " + << (lag_expiration_ - min_stamp) << " seconds too old. Ignoring this transaction."); + return false; } + return true; +} - void FixedLagSmoother::onReset() - { - std::lock_guard lock(mutex_); - timestamp_tracking_.clear(); - lag_expiration_ = ros::Time(0, 0); - } +void FixedLagSmoother::onReset() +{ + std::lock_guard lock(mutex_); + timestamp_tracking_.clear(); + lag_expiration_ = ros::Time(0, 0); +} } // namespace fuse_optimizers From 33faaaf18f4c9e43f96b30b5d054e09548c214bd Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 18 Jan 2023 16:35:50 -0500 Subject: [PATCH 16/17] formatting --- fuse_optimizers/src/fixed_lag_smoother.cpp | 88 +++++++++------------- 1 file changed, 36 insertions(+), 52 deletions(-) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index f6584970a..8b709b6ae 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -64,69 +64,53 @@ namespace fuse_optimizers { } - FixedLagSmoother::FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle &node_handle, - const ros::NodeHandle &private_node_handle) : FixedLagSmoother::FixedLagSmoother(std::move(graph), - ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), - node_handle, - private_node_handle) - { - } - void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction &new_transaction) { std::lock_guard lock(mutex_); timestamp_tracking_.addNewTransaction(new_transaction); } - std::lock_guard lock(mutex_); - auto now = timestamp_tracking_[timestamp_tracking_.numStates() - 1]; - lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; - auto marginalize_variable_uuids = std::vector(); - timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); - return marginalize_variable_uuids; -} + std::vector FixedLagSmoother::computeVariablesToMarginalize() + { + // Find the most recent variable timestamp, then carefully subtract the lag duration. + // ROS Time objects do not handle negative values. + auto start_time = getStartTime(); -std::lock_guard lock(mutex_); -auto now = timestamp_tracking_.currentStamp(); -lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; -auto marginalize_variable_uuids = std::vector(); -timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); -return marginalize_variable_uuids; -} + std::lock_guard lock(mutex_); + auto now = timestamp_tracking_[timestamp_tracking_.numStates() - 1]; + lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time; + auto marginalize_variable_uuids = std::vector(); + timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids)); + return marginalize_variable_uuids; + } -void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction) -{ - ROS_DEBUG_STREAM("The current lag expiration time is " - << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() - << " from sensor " << sensor_name << " has a minimum involved timestamp of " << min_stamp - << ", which is " << (lag_expiration_ - min_stamp) - << " seconds too old. Ignoring this transaction."); - return false; -} + void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction) + { + std::lock_guard lock(mutex_); + timestamp_tracking_.addMarginalTransaction(marginal_transaction); + } -bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction) -{ - auto min_stamp = transaction.minStamp(); - std::lock_guard lock(mutex_); - if (min_stamp < lag_expiration_) + bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction) { - ROS_DEBUG_STREAM( - "The current lag expiration time is " - << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " - << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is " - << (lag_expiration_ - min_stamp) << " seconds too old. Ignoring this transaction."); - return false; + auto min_stamp = transaction.minStamp(); + std::lock_guard lock(mutex_); + if (min_stamp < lag_expiration_) + { + ROS_DEBUG_STREAM( + "The current lag expiration time is " + << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " + << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is " + << (lag_expiration_ - min_stamp) << " seconds too old. Ignoring this transaction."); + return false; + } + return true; } - return true; -} -void FixedLagSmoother::onReset() -{ - std::lock_guard lock(mutex_); - timestamp_tracking_.clear(); - lag_expiration_ = ros::Time(0, 0); -} + void FixedLagSmoother::onReset() + { + std::lock_guard lock(mutex_); + timestamp_tracking_.clear(); + lag_expiration_ = ros::Time(0, 0); + } } // namespace fuse_optimizers From 5a3e36f674c6a2699d52fe4ed65754e225c2f2d8 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 18 Jan 2023 16:56:31 -0500 Subject: [PATCH 17/17] Remove marginalization fix --- fuse_optimizers/CMakeLists.txt | 1 - fuse_optimizers/src/windowed_optimizer.cpp | 915 ++++++++++----------- 2 files changed, 444 insertions(+), 472 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 1f8bcad62..1edea2c38 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -1,6 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") set(build_depends diagnostic_updater diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp index 184b0b745..6edc795a6 100644 --- a/fuse_optimizers/src/windowed_optimizer.cpp +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -50,519 +50,492 @@ namespace { -/** - * @brief Delete an element from the vector using a reverse iterator - * - * @param[in] container The contain to delete from - * @param[in] position A reverse iterator that access the element to be erased - * @return A reverse iterator pointing to the element after the erased element - */ -template -typename std::vector::reverse_iterator erase(std::vector& container, - typename std::vector::reverse_iterator position) -{ - // Reverse iterators are weird - // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator - // Basically a reverse iterator access the element one place before the element it points at. - // E.g. The reverse iterator rbegin points at end, but accesses end-1. - // When you delete something, you need to increment the reverse iterator first, then convert it to a standard - // iterator for the erase operation. - std::advance(position, 1); - container.erase(position.base()); - return position; -} -} // namespace + /** + * @brief Delete an element from the vector using a reverse iterator + * + * @param[in] container The contain to delete from + * @param[in] position A reverse iterator that access the element to be erased + * @return A reverse iterator pointing to the element after the erased element + */ + template + typename std::vector::reverse_iterator erase(std::vector &container, + typename std::vector::reverse_iterator position) + { + // Reverse iterators are weird + // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator + // Basically a reverse iterator access the element one place before the element it points at. + // E.g. The reverse iterator rbegin points at end, but accesses end-1. + // When you delete something, you need to increment the reverse iterator first, then convert it to a standard + // iterator for the erase operation. + std::advance(position, 1); + container.erase(position.base()); + return position; + } +} // namespace namespace fuse_optimizers { -WindowedOptimizer::WindowedOptimizer(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) - : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) - , params_(params) - , ignited_(false) - , optimization_running_(true) - , started_(false) - , optimization_request_(false) -{ - // Test for auto-start - autostart(); - - // Start the optimization thread - optimization_thread_ = std::thread(&WindowedOptimizer::optimizationLoop, this); + WindowedOptimizer::WindowedOptimizer(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr ¶ms, + const ros::NodeHandle &node_handle, const ros::NodeHandle &private_node_handle) + : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), params_(params), ignited_(false), optimization_running_(true), started_(false), optimization_request_(false) + { + // Test for auto-start + autostart(); - // Configure a timer to trigger optimizations - optimize_timer_ = - node_handle_.createTimer(params_->optimization_period, &WindowedOptimizer::optimizerTimerCallback, this); + // Start the optimization thread + optimization_thread_ = std::thread(&WindowedOptimizer::optimizationLoop, this); - // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = node_handle_.advertiseService(ros::names::resolve(params_->reset_service), - &WindowedOptimizer::resetServiceCallback, this); + // Configure a timer to trigger optimizations + optimize_timer_ = + node_handle_.createTimer(params_->optimization_period, &WindowedOptimizer::optimizerTimerCallback, this); - // Subscribe to a reset message that will reset the optimizer to the initial state - reset_subscriber_ = node_handle_.subscribe(ros::names::resolve(params_->reset_service), 10, - &WindowedOptimizer::resetMessageCallback, this); -} + // Advertise a service that resets the optimizer to its initial state + reset_service_server_ = node_handle_.advertiseService(ros::names::resolve(params_->reset_service), + &WindowedOptimizer::resetServiceCallback, this); -WindowedOptimizer::~WindowedOptimizer() -{ - // Wake up any sleeping threads - optimization_running_ = false; - optimization_requested_.notify_all(); - // Wait for the threads to shutdown - if (optimization_thread_.joinable()) - { - optimization_thread_.join(); + // Subscribe to a reset message that will reset the optimizer to the initial state + reset_subscriber_ = node_handle_.subscribe(ros::names::resolve(params_->reset_service), 10, + &WindowedOptimizer::resetMessageCallback, this); } -} -void WindowedOptimizer::autostart() -{ - if (std::none_of(sensor_models_.begin(), sensor_models_.end(), - [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) + WindowedOptimizer::~WindowedOptimizer() { - // No ignition sensors were provided. Auto-start. - started_ = true; - setStartTime(ros::Time(0, 0)); - ROS_INFO_STREAM("No ignition sensors were specified. Optimization will begin immediately."); + // Wake up any sleeping threads + optimization_running_ = false; + optimization_requested_.notify_all(); + // Wait for the threads to shutdown + if (optimization_thread_.joinable()) + { + optimization_thread_.join(); + } } -} -void WindowedOptimizer::optimizationLoop() -{ - auto exit_wait_condition = [this]() { - return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); - }; - // Optimize constraints until told to exit - while (ros::ok() && optimization_running_) + void WindowedOptimizer::autostart() { - // Wait for the next signal to start the next optimization cycle - auto optimization_deadline = ros::Time(0, 0); + if (std::none_of(sensor_models_.begin(), sensor_models_.end(), + [](const auto &element) + { return element.second.ignition; })) // NOLINT(whitespace/braces) { - std::unique_lock lock(optimization_requested_mutex_); - optimization_requested_.wait(lock, exit_wait_condition); - optimization_request_ = false; - optimization_deadline = optimization_deadline_; + // No ignition sensors were provided. Auto-start. + started_ = true; + setStartTime(ros::Time(0, 0)); + ROS_INFO_STREAM("No ignition sensors were specified. Optimization will begin immediately."); } - // If a shutdown is requested, exit now. - if (!optimization_running_ || !ros::ok()) + } + + void WindowedOptimizer::optimizationLoop() + { + auto exit_wait_condition = [this]() { - break; - } - // Optimize + return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); + }; + // Optimize constraints until told to exit + while (ros::ok() && optimization_running_) { - std::lock_guard lock(optimization_mutex_); - // Apply motion models - auto new_transaction = fuse_core::Transaction::make_shared(); - // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ - // We do this to ensure state of the graph does not change between unlocking the pending_transactions - // queue and obtaining the lock for the graph. But we have now obtained two different locks. If we are - // not extremely careful, we could get a deadlock. - processQueue(*new_transaction); - // Skip this optimization cycle if the transaction is empty because something failed while processing the pending - // transactions queue. - if (new_transaction->empty()) + // Wait for the next signal to start the next optimization cycle + auto optimization_deadline = ros::Time(0, 0); { - continue; + std::unique_lock lock(optimization_requested_mutex_); + optimization_requested_.wait(lock, exit_wait_condition); + optimization_request_ = false; + optimization_deadline = optimization_deadline_; } - // check if new transaction has added constraints to variables that are to be marginalized - std::vector faulty_constraints; - for (auto& c : new_transaction->addedConstraints()) + // If a shutdown is requested, exit now. + if (!optimization_running_ || !ros::ok()) { - for (auto var_uuid : c.variables()) - { - for (auto marginal_uuid : marginal_transaction_.removedVariables()) - { - if (var_uuid == marginal_uuid) - { - faulty_constraints.push_back(c.uuid()); - break; - } - } - } + break; } - if (faulty_constraints.size() > 0) + // Optimize { - ROS_WARN_STREAM("Removing invalid constraints."); - for (auto& faulty_constraint : faulty_constraints) + std::lock_guard lock(optimization_mutex_); + // Apply motion models + auto new_transaction = fuse_core::Transaction::make_shared(); + // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ + // We do this to ensure state of the graph does not change between unlocking the pending_transactions + // queue and obtaining the lock for the graph. But we have now obtained two different locks. If we are + // not extremely careful, we could get a deadlock. + processQueue(*new_transaction); + // Skip this optimization cycle if the transaction is empty because something failed while processing the pending + // transactions queue. + if (new_transaction->empty()) { - new_transaction->removeConstraint(faulty_constraint); + continue; } - } - - // Prepare for selecting the marginal variables -- Delegated to derived classes - preprocessMarginalization(*new_transaction); - // Combine the new transactions with any marginal transaction from the end of the last cycle - new_transaction->merge(marginal_transaction_); - // Update the graph - try - { - graph_->update(*new_transaction); - } - catch (const std::exception& ex) - { - std::ostringstream oss; - oss << "Graph:\n"; - graph_->print(oss); - oss << "\nTransaction:\n"; - new_transaction->print(oss); - - ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() - << "\nLeaving optimization loop and requesting " - "node shutdown...\n" - << oss.str()); - ros::requestShutdown(); - break; - } - // Optimize the entire graph - summary_ = graph_->optimize(params_->solver_options); + // Prepare for selecting the marginal variables -- Delegated to derived classes + preprocessMarginalization(*new_transaction); + // Combine the new transactions with any marginal transaction from the end of the last cycle + new_transaction->merge(marginal_transaction_); + // Update the graph + try + { + graph_->update(*new_transaction); + } + catch (const std::exception &ex) + { + std::ostringstream oss; + oss << "Graph:\n"; + graph_->print(oss); + oss << "\nTransaction:\n"; + new_transaction->print(oss); + + ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() + << "\nLeaving optimization loop and requesting " + "node shutdown...\n" + << oss.str()); + ros::requestShutdown(); + break; + } + // Optimize the entire graph + summary_ = graph_->optimize(params_->solver_options); - // Optimization is complete. Notify all the things about the graph changes. - const auto new_transaction_stamp = new_transaction->stamp(); - notify(std::move(new_transaction), graph_->clone()); + // Optimization is complete. Notify all the things about the graph changes. + const auto new_transaction_stamp = new_transaction->stamp(); + notify(std::move(new_transaction), graph_->clone()); - // Abort if optimization failed. Not converging is not a failure because the solution found is usable. - if (!summary_.IsSolutionUsable()) - { - ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); - ROS_INFO_STREAM(summary_.FullReport()); - ros::requestShutdown(); - break; - } + // Abort if optimization failed. Not converging is not a failure because the solution found is usable. + if (!summary_.IsSolutionUsable()) + { + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + ROS_INFO_STREAM(summary_.FullReport()); + ros::requestShutdown(); + break; + } - // Marginal out any variables outside of the current "window" - // Determination of which variables to marginalize is delegated to derived classes - auto variables_to_marginalize = computeVariablesToMarginalize(); - marginal_transaction_ = - fuse_constraints::marginalizeVariables(ros::this_node::getName(), variables_to_marginalize, *graph_); - // Perform any post-marginal cleanup -- Delegated to derived classes - postprocessMarginalization(marginal_transaction_); - // Note: The marginal transaction will not be applied until the next optimization iteration - // Log a warning if the optimization took too long - auto optimization_complete = ros::Time::now(); - if (optimization_complete > optimization_deadline) - { - ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " - << (optimization_complete - optimization_deadline) << "s"); + // Marginal out any variables outside of the current "window" + // Determination of which variables to marginalize is delegated to derived classes + auto variables_to_marginalize = computeVariablesToMarginalize(); + marginal_transaction_ = + fuse_constraints::marginalizeVariables(ros::this_node::getName(), variables_to_marginalize, *graph_); + // Perform any post-marginal cleanup -- Delegated to derived classes + postprocessMarginalization(marginal_transaction_); + // Note: The marginal transaction will not be applied until the next optimization iteration + // Log a warning if the optimization took too long + auto optimization_complete = ros::Time::now(); + if (optimization_complete > optimization_deadline) + { + ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline) << "s"); + } } } } -} -void WindowedOptimizer::optimizerTimerCallback(const ros::TimerEvent& event) -{ - // If an "ignition" transaction hasn't been received, then we can't do anything yet. - if (!started_) - { - return; - } - // If there is some pending work, trigger the next optimization cycle. - // If the optimizer has not completed the previous optimization cycle, then it - // will not be waiting on the condition variable signal, so nothing will happen. - auto optimization_request = false; - { - std::lock_guard lock(pending_transactions_mutex_); - optimization_request = !pending_transactions_.empty(); - } - if (optimization_request) + void WindowedOptimizer::optimizerTimerCallback(const ros::TimerEvent &event) { + // If an "ignition" transaction hasn't been received, then we can't do anything yet. + if (!started_) { - std::lock_guard lock(optimization_requested_mutex_); - optimization_request_ = true; - optimization_deadline_ = event.current_expected + params_->optimization_period; + return; + } + // If there is some pending work, trigger the next optimization cycle. + // If the optimizer has not completed the previous optimization cycle, then it + // will not be waiting on the condition variable signal, so nothing will happen. + auto optimization_request = false; + { + std::lock_guard lock(pending_transactions_mutex_); + optimization_request = !pending_transactions_.empty(); + } + if (optimization_request) + { + { + std::lock_guard lock(optimization_requested_mutex_); + optimization_request_ = true; + optimization_deadline_ = event.current_expected + params_->optimization_period; + } + optimization_requested_.notify_one(); } - optimization_requested_.notify_one(); - } -} - -void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) -{ - // We need to get the pending transactions from the queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - - if (pending_transactions_.empty()) - { - return; } - // If we just started because an ignition sensor transaction was received, we try to process it individually. This is - // important because we need to update the graph with the ignition sensor transaction in order to get the motion - // models notified of the initial state. The motion models will typically maintain a state history in order to create - // motion model constraints with the optimized variables from the updated graph. If we do not process the ignition - // sensor transaction individually, the motion model constraints created for the other queued transactions will not be - // able to use any optimized variables from the graph because it is not been optimized yet, and they will have to use - // a default zero state instead. This can easily lead to local minima because the variables in the graph are not - // initialized properly, i.e. they do not take the ignition sensor transaction into account. - if (ignited_) + void WindowedOptimizer::processQueue(fuse_core::Transaction &transaction) { - // The ignition sensor transaction is assumed to be at the end of the queue, because it must be the oldest one. - // If there is more than one ignition sensor transaction in the queue, it is always the oldest one that started - // things up. - ignited_ = false; + // We need to get the pending transactions from the queue + std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - const auto transaction_rbegin = pending_transactions_.rbegin(); - auto& element = *transaction_rbegin; - if (!sensor_models_.at(element.sensor_name).ignition) + if (pending_transactions_.empty()) { - // We just started, but the oldest transaction is not from an ignition sensor. We will still process the - // transaction, but we do not enforce it is processed individually. - ROS_ERROR_STREAM("The queued transaction with timestamp " - << element.stamp() << " from sensor " << element.sensor_name - << " is not an ignition sensor transaction. " - << "This transaction will not be processed individually."); + return; } - else + + // If we just started because an ignition sensor transaction was received, we try to process it individually. This is + // important because we need to update the graph with the ignition sensor transaction in order to get the motion + // models notified of the initial state. The motion models will typically maintain a state history in order to create + // motion model constraints with the optimized variables from the updated graph. If we do not process the ignition + // sensor transaction individually, the motion model constraints created for the other queued transactions will not be + // able to use any optimized variables from the graph because it is not been optimized yet, and they will have to use + // a default zero state instead. This can easily lead to local minima because the variables in the graph are not + // initialized properly, i.e. they do not take the ignition sensor transaction into account. + if (ignited_) { - if (applyMotionModels(element.sensor_name, *element.transaction)) + // The ignition sensor transaction is assumed to be at the end of the queue, because it must be the oldest one. + // If there is more than one ignition sensor transaction in the queue, it is always the oldest one that started + // things up. + ignited_ = false; + + const auto transaction_rbegin = pending_transactions_.rbegin(); + auto &element = *transaction_rbegin; + if (!sensor_models_.at(element.sensor_name).ignition) { - // Processing was successful. Add the results to the final transaction, delete this one, and return, so the - // transaction from the ignition sensor is processed individually. - transaction.merge(*element.transaction, true); - erase(pending_transactions_, transaction_rbegin); + // We just started, but the oldest transaction is not from an ignition sensor. We will still process the + // transaction, but we do not enforce it is processed individually. + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.stamp() << " from sensor " << element.sensor_name + << " is not an ignition sensor transaction. " + << "This transaction will not be processed individually."); } else { - // The motion model processing failed. When this happens to an ignition sensor transaction there is no point on - // trying again next time, so we ignore this transaction. - ROS_ERROR_STREAM("The queued ignition transaction with timestamp " - << element.stamp() << " from sensor " << element.sensor_name - << " could not be processed. Ignoring this ignition " - "transaction."); - - // Remove the ignition transaction that just failed and purge all transactions after it. But if we find another - // ignition transaction, we schedule it to be processed in the next optimization cycle. - erase(pending_transactions_, transaction_rbegin); - - const auto pending_ignition_transaction_iter = - std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), - [this](const auto& element) { // NOLINT(whitespace/braces) - return sensor_models_.at(element.sensor_name).ignition; - }); // NOLINT(whitespace/braces) - if (pending_ignition_transaction_iter == pending_transactions_.rend()) + if (applyMotionModels(element.sensor_name, *element.transaction)) { - // There is no other ignition transaction pending. We simply roll back to not started state and all other - // pending transactions will be handled later in the transaction callback, as usual. - started_ = false; + // Processing was successful. Add the results to the final transaction, delete this one, and return, so the + // transaction from the ignition sensor is processed individually. + transaction.merge(*element.transaction, true); + erase(pending_transactions_, transaction_rbegin); } else { - // Erase all transactions before the other ignition transaction pending. This other ignition transaction will - // be processed in the next optimization cycle. - pending_transactions_.erase(pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); - ignited_ = true; + // The motion model processing failed. When this happens to an ignition sensor transaction there is no point on + // trying again next time, so we ignore this transaction. + ROS_ERROR_STREAM("The queued ignition transaction with timestamp " + << element.stamp() << " from sensor " << element.sensor_name + << " could not be processed. Ignoring this ignition " + "transaction."); + + // Remove the ignition transaction that just failed and purge all transactions after it. But if we find another + // ignition transaction, we schedule it to be processed in the next optimization cycle. + erase(pending_transactions_, transaction_rbegin); + + const auto pending_ignition_transaction_iter = + std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), + [this](const auto &element) { // NOLINT(whitespace/braces) + return sensor_models_.at(element.sensor_name).ignition; + }); // NOLINT(whitespace/braces) + if (pending_ignition_transaction_iter == pending_transactions_.rend()) + { + // There is no other ignition transaction pending. We simply roll back to not started state and all other + // pending transactions will be handled later in the transaction callback, as usual. + started_ = false; + } + else + { + // Erase all transactions before the other ignition transaction pending. This other ignition transaction will + // be processed in the next optimization cycle. + pending_transactions_.erase(pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); + ignited_ = true; + } } - } - // There are no more pending transactions to process in this optimization cycle, or they should be processed in - // the next one. - return; + // There are no more pending transactions to process in this optimization cycle, or they should be processed in + // the next one. + return; + } } - } - // Use the most recent transaction time as the current time - const auto current_time = pending_transactions_.front().stamp(); + // Use the most recent transaction time as the current time + const auto current_time = pending_transactions_.front().stamp(); - // Attempt to process each pending transaction - auto sensor_blacklist = std::vector(); - auto transaction_riter = pending_transactions_.rbegin(); - while (transaction_riter != pending_transactions_.rend()) - { - auto& element = *transaction_riter; - if (!validateTransaction(element.sensor_name, *element.transaction)) - { - transaction_riter = erase(pending_transactions_, transaction_riter); - } - else if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) + // Attempt to process each pending transaction + auto sensor_blacklist = std::vector(); + auto transaction_riter = pending_transactions_.rbegin(); + while (transaction_riter != pending_transactions_.rend()) { - // We should not process transactions from this sensor - ++transaction_riter; - } - else if (applyMotionModels(element.sensor_name, *element.transaction)) - { - // Processing was successful. Add the results to the final transaction, delete this one, and move to the next. - transaction.merge(*element.transaction, true); - transaction_riter = erase(pending_transactions_, transaction_riter); - } - else - { - // The motion model processing failed. - // Check the transaction timeout to determine if it should be removed or skipped. - const auto& max_stamp = element.maxStamp(); - if (max_stamp + params_->transaction_timeout < current_time) + auto &element = *transaction_riter; + if (!validateTransaction(element.sensor_name, *element.transaction)) { - // Warn that this transaction has expired, then skip it. - ROS_ERROR_STREAM("The queued transaction with timestamp " - << element.stamp() << " and maximum " - "involved stamp of " - << max_stamp << " from sensor " << element.sensor_name << " could not be processed after " - << (current_time - max_stamp) << " seconds, " - "which is greater than the 'transaction_timeout' value of " - << params_->transaction_timeout << ". Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); } - else + else if (std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) { - // The motion model failed. Stop further processing of this sensor and try again next time. - sensor_blacklist.push_back(element.sensor_name); + // We should not process transactions from this sensor ++transaction_riter; } + else if (applyMotionModels(element.sensor_name, *element.transaction)) + { + // Processing was successful. Add the results to the final transaction, delete this one, and move to the next. + transaction.merge(*element.transaction, true); + transaction_riter = erase(pending_transactions_, transaction_riter); + } + else + { + // The motion model processing failed. + // Check the transaction timeout to determine if it should be removed or skipped. + const auto &max_stamp = element.maxStamp(); + if (max_stamp + params_->transaction_timeout < current_time) + { + // Warn that this transaction has expired, then skip it. + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.stamp() << " and maximum " + "involved stamp of " + << max_stamp << " from sensor " << element.sensor_name << " could not be processed after " + << (current_time - max_stamp) << " seconds, " + "which is greater than the 'transaction_timeout' value of " + << params_->transaction_timeout << ". Ignoring this transaction."); + transaction_riter = erase(pending_transactions_, transaction_riter); + } + else + { + // The motion model failed. Stop further processing of this sensor and try again next time. + sensor_blacklist.push_back(element.sensor_name); + ++transaction_riter; + } + } } } -} -void WindowedOptimizer::reset() -{ - // Tell all the plugins to stop - stopPlugins(); - // Reset the optimizer state - { - std::lock_guard lock(optimization_requested_mutex_); - optimization_request_ = false; - } - started_ = false; - ignited_ = false; - setStartTime(ros::Time(0, 0)); - // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the - // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to - // prevent the possibility of deadlocks. + void WindowedOptimizer::reset() { - std::lock_guard lock(optimization_mutex_); - // Clear all pending transactions + // Tell all the plugins to stop + stopPlugins(); + // Reset the optimizer state { - std::lock_guard lock(pending_transactions_mutex_); - pending_transactions_.clear(); + std::lock_guard lock(optimization_requested_mutex_); + optimization_request_ = false; } - // Clear the graph and marginal tracking states - graph_->clear(); - marginal_transaction_ = fuse_core::Transaction(); + started_ = false; + ignited_ = false; + setStartTime(ros::Time(0, 0)); + // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the + // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to + // prevent the possibility of deadlocks. + { + std::lock_guard lock(optimization_mutex_); + // Clear all pending transactions + { + std::lock_guard lock(pending_transactions_mutex_); + pending_transactions_.clear(); + } + // Clear the graph and marginal tracking states + graph_->clear(); + marginal_transaction_ = fuse_core::Transaction(); + } + // Perform any required reset operations for derived classes + onReset(); + // Tell all the plugins to start + startPlugins(); + // Test for auto-start + autostart(); + return; } - // Perform any required reset operations for derived classes - onReset(); - // Tell all the plugins to start - startPlugins(); - // Test for auto-start - autostart(); - return; -} - -void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&) -{ - // perform reset logic - reset(); - return; -} - -bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - // perform reset logic - reset(); - return true; -} -void WindowedOptimizer::transactionCallback(const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) -{ - // If this transaction occurs before the start time, just ignore it - auto start_time = getStartTime(); - const auto max_time = transaction->maxStamp(); - if (started_ && max_time < start_time) + void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr &) { - ROS_DEBUG_STREAM("Received a transaction before the start time from sensor '" - << sensor_name << "'.\n" - << " start_time: " << start_time << ", maximum involved stamp: " << max_time - << ", difference: " << (start_time - max_time) << "s"); + // perform reset logic + reset(); return; } - { - // We need to add the new transaction to the pending_transactions_ queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - // Add the new transaction to the pending set - // The pending set is arranged "smallest stamp last" to making popping off the back more efficient - auto comparator = [](const ros::Time& value, const TransactionQueueElement& element) { - return value >= element.stamp(); - }; - auto position = - std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); - position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT + bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &) + { + // perform reset logic + reset(); + return true; + } - // If we haven't "started" yet.. - if (!started_) + void WindowedOptimizer::transactionCallback(const std::string &sensor_name, + fuse_core::Transaction::SharedPtr transaction) + { + // If this transaction occurs before the start time, just ignore it + auto start_time = getStartTime(); + const auto max_time = transaction->maxStamp(); + if (started_ && max_time < start_time) + { + ROS_DEBUG_STREAM("Received a transaction before the start time from sensor '" + << sensor_name << "'.\n" + << " start_time: " << start_time << ", maximum involved stamp: " << max_time + << ", difference: " << (start_time - max_time) << "s"); + return; + } { - // ...check if we should - if (sensor_models_.at(sensor_name).ignition) + // We need to add the new transaction to the pending_transactions_ queue + std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + + // Add the new transaction to the pending set + // The pending set is arranged "smallest stamp last" to making popping off the back more efficient + auto comparator = [](const ros::Time &value, const TransactionQueueElement &element) { - started_ = true; - ignited_ = true; - start_time = position->minStamp(); - setStartTime(start_time); - - // And purge out old transactions - // - Either before or exactly at the start time - // - Or with a minimum time before the minimum time of this ignition sensor transaction - // - // TODO(efernandez) Do '&min_time = std::as_const(start_time)' when C++17 is supported and we can use - // std::as_const: https://en.cppreference.com/w/cpp/utility/as_const - pending_transactions_.erase( - std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, - &min_time = start_time ](const auto& transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); - } - else + return value >= element.stamp(); + }; + auto position = + std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); + position = pending_transactions_.insert(position, {sensor_name, std::move(transaction)}); // NOLINT + + // If we haven't "started" yet.. + if (!started_) { - // And purge out old transactions to limit the pending size while waiting for an ignition sensor - auto purge_time = ros::Time(0, 0); - auto last_pending_time = pending_transactions_.front().stamp(); - if (ros::Time(0, 0) + params_->transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives + // ...check if we should + if (sensor_models_.at(sensor_name).ignition) { - purge_time = last_pending_time - params_->transaction_timeout; + started_ = true; + ignited_ = true; + start_time = position->minStamp(); + setStartTime(start_time); + + // And purge out old transactions + // - Either before or exactly at the start time + // - Or with a minimum time before the minimum time of this ignition sensor transaction + // + // TODO(efernandez) Do '&min_time = std::as_const(start_time)' when C++17 is supported and we can use + // std::as_const: https://en.cppreference.com/w/cpp/utility/as_const + pending_transactions_.erase( + std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), + [&sensor_name, max_time, + &min_time = start_time](const auto &transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); } - - while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) + else { - pending_transactions_.pop_back(); + // And purge out old transactions to limit the pending size while waiting for an ignition sensor + auto purge_time = ros::Time(0, 0); + auto last_pending_time = pending_transactions_.front().stamp(); + if (ros::Time(0, 0) + params_->transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives + { + purge_time = last_pending_time - params_->transaction_timeout; + } + + while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) + { + pending_transactions_.pop_back(); + } } } } } -} -/** - * @brief Make a diagnostic_msgs::DiagnosticStatus message filling in the level and message - * - * @param[in] level The diagnostic status level - * @param[in] message The diagnostic status message - */ -diagnostic_msgs::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string& message) -{ - diagnostic_msgs::DiagnosticStatus status; + /** + * @brief Make a diagnostic_msgs::DiagnosticStatus message filling in the level and message + * + * @param[in] level The diagnostic status level + * @param[in] message The diagnostic status message + */ + diagnostic_msgs::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string &message) + { + diagnostic_msgs::DiagnosticStatus status; - status.level = level; - status.message = message; + status.level = level; + status.message = message; - return status; -} + return status; + } -/** - * @brief Helper function to generate the diagnostic status for each optimization termination type - * - * The termination type -> diagnostic status mapping is as follows: - * - * - CONVERGENCE, USER_SUCCESS -> OK - * - NO_CONVERGENCE -> WARN - * - FAILURE, USER_FAILURE -> ERROR (default) - * - * @param[in] termination_type The optimization termination type - * @return The diagnostic status with the level and message corresponding to the optimization termination type - */ -diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type) -{ - switch (termination_type) + /** + * @brief Helper function to generate the diagnostic status for each optimization termination type + * + * The termination type -> diagnostic status mapping is as follows: + * + * - CONVERGENCE, USER_SUCCESS -> OK + * - NO_CONVERGENCE -> WARN + * - FAILURE, USER_FAILURE -> ERROR (default) + * + * @param[in] termination_type The optimization termination type + * @return The diagnostic status with the level and message corresponding to the optimization termination type + */ + diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type) { + switch (termination_type) + { case ceres::TerminationType::CONVERGENCE: case ceres::TerminationType::USER_SUCCESS: return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::OK, "Optimization converged"); @@ -570,67 +543,67 @@ diagnostic_msgs::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres: return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::WARN, "Optimization didn't converge"); default: return makeDiagnosticStatus(diagnostic_msgs::DiagnosticStatus::ERROR, "Optimization failed"); + } } -} -void WindowedOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) -{ - Optimizer::setDiagnostics(status); + void WindowedOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &status) + { + Optimizer::setDiagnostics(status); - // Load std::atomic flag that indicates whether the optimizer has started or not - const bool started = started_; + // Load std::atomic flag that indicates whether the optimizer has started or not + const bool started = started_; - status.add("Started", started); - { - std::lock_guard lock(pending_transactions_mutex_); - status.add("Pending Transactions", pending_transactions_.size()); - } + status.add("Started", started); + { + std::lock_guard lock(pending_transactions_mutex_); + status.add("Pending Transactions", pending_transactions_.size()); + } - if (started) - { - // Add some optimization summary report fields to the diagnostics status if the optimizer has started - auto summary = decltype(summary_)(); + if (started) { - const std::unique_lock lock(optimization_mutex_, std::try_to_lock); - if (lock) - { - summary = summary_; - } - else + // Add some optimization summary report fields to the diagnostics status if the optimizer has started + auto summary = decltype(summary_)(); { - status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimization running"); + const std::unique_lock lock(optimization_mutex_, std::try_to_lock); + if (lock) + { + summary = summary_; + } + else + { + status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Optimization running"); + } } - } - if (summary.total_time_in_seconds >= 0.0) // This is -1 for the default-constructed summary object - { - status.add("Optimization Termination Type", ceres::TerminationTypeToString(summary.termination_type)); - status.add("Optimization Total Time [s]", summary.total_time_in_seconds); - status.add("Optimization Iterations", summary.iterations.size()); - status.add("Initial Cost", summary.initial_cost); - status.add("Final Cost", summary.final_cost); + if (summary.total_time_in_seconds >= 0.0) // This is -1 for the default-constructed summary object + { + status.add("Optimization Termination Type", ceres::TerminationTypeToString(summary.termination_type)); + status.add("Optimization Total Time [s]", summary.total_time_in_seconds); + status.add("Optimization Iterations", summary.iterations.size()); + status.add("Initial Cost", summary.initial_cost); + status.add("Final Cost", summary.final_cost); - status.mergeSummary(terminationTypeToDiagnosticStatus(summary.termination_type)); - } + status.mergeSummary(terminationTypeToDiagnosticStatus(summary.termination_type)); + } - // Add time since the last optimization request time. This is useful to detect if no transactions are received for - // too long - auto optimization_deadline = decltype(optimization_deadline_)(); - { - const std::unique_lock lock(optimization_requested_mutex_, std::try_to_lock); - if (lock) + // Add time since the last optimization request time. This is useful to detect if no transactions are received for + // too long + auto optimization_deadline = decltype(optimization_deadline_)(); { - optimization_deadline = optimization_deadline_; + const std::unique_lock lock(optimization_requested_mutex_, std::try_to_lock); + if (lock) + { + optimization_deadline = optimization_deadline_; + } } - } - if (!optimization_deadline.isZero()) // This is zero for the default-constructed optimization_deadline object - { - const auto optimization_request_time = optimization_deadline - params_->optimization_period; - const auto time_since_last_optimization_request = ros::Time::now() - optimization_request_time; - status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.toSec()); + if (!optimization_deadline.isZero()) // This is zero for the default-constructed optimization_deadline object + { + const auto optimization_request_time = optimization_deadline - params_->optimization_period; + const auto time_since_last_optimization_request = ros::Time::now() - optimization_request_time; + status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.toSec()); + } } } -} -} // namespace fuse_optimizers +} // namespace fuse_optimizers