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/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 798787394..1edea2c38 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -12,6 +12,8 @@ set(build_depends std_srvs ) +find_package(OpenMP) + find_package(catkin REQUIRED COMPONENTS ${build_depends} ) @@ -34,7 +36,9 @@ 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 ) add_dependencies(${PROJECT_NAME} @@ -47,12 +51,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 @@ -98,6 +106,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/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 da7d56ad5..3df84e8e1 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 * @@ -102,7 +97,7 @@ 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); @@ -113,88 +108,36 @@ class FixedLagSmoother : public Optimizer * * @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 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 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. + * @brief Constructor * - * 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. + * The parameters will be loaded from the ROS parameter server using the private node handle * - * Also, we sort the queue with the smallest stamp last. This allows us to clear the queue using the more - * efficient pop_back() operation. + * @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 */ - using TransactionQueue = std::vector; + 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 - 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 + ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-lag smoother - // 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 - 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 + // 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 - 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 - */ - void autostart(); /** * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called @@ -206,12 +149,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 +160,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 +171,22 @@ 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); + void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) override; /** - * @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. + * @brief Determine if a new transaction should be applied to the graph * - * This implementation shares ownership of the transaction object. + * Test if the transaction is within the defined lag window of the smoother. * - * @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 + * @param[in] sensor_name - The name of the sensor that produced the provided transaction + * @param[in] transaction - The transaction to be validated */ - void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + bool validateTransaction(const std::string& sensor_name, const fuse_core::Transaction& 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..4e78734d1 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 @@ -45,47 +46,20 @@ #include #include - namespace fuse_optimizers { - /** * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class */ -struct FixedLagSmootherParams +struct FixedLagSmootherParams : public WindowedOptimizerParams { public: - /** - * @brief The duration of the smoothing window in seconds - */ - 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 }; + SMART_PTR_DEFINITIONS(FixedLagSmootherParams); /** - * @brief Ceres Solver::Options object that controls various aspects of the optimizer. + * @brief The duration of the smoothing window in seconds */ - ceres::Solver::Options solver_options; + ros::Duration lag_duration{ 5.0 }; /** * @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/fixed_size_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h new file mode 100644 index 000000000..7498dff19 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h @@ -0,0 +1,193 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, 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-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. + * + * 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 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 + * 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: + * - 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) + * 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..fb2c795a5 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h @@ -0,0 +1,72 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, 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 + +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 Thenumber of unique stamps in the window + */ + int 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/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 f0342f9dd..cb977921f 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h @@ -71,12 +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 + */ + int numStates() const + { + return unique_stamps_.size(); + } /** * @brief Clear all tracked state @@ -86,12 +100,15 @@ class VariableStampIndex stamped_index_.clear(); variables_.clear(); constraints_.clear(); + unique_stamps_.clear(); } /** - * @brief Returns the most recent timestamp associated with any variable + * @brief Provides a random access operator the the unique timestamps + * + * @param[in] i index to access, if negative Time(0) is returned */ - ros::Time currentStamp() const; + ros::Time operator[](int i); /** * @brief Update the index with the information from the added transactions @@ -176,6 +193,9 @@ class VariableStampIndex using ConstraintToVariablesMap = std::unordered_map>; ConstraintToVariablesMap constraints_; + 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 new file mode 100644 index 000000000..6c4598204 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h @@ -0,0 +1,348 @@ +/* + * 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 +#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; + + /** + * @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::SharedPtr& params, + 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::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 + //!< 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 + ros::Subscriber reset_subscriber_; //!< Subscriber 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. + * + * 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; + + /** + * @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. + * + * 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; + + /** + * @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. + * + * 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; + + /** + * @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 Performs all logic when the client calls for a reset + */ + void reset(); + + /** + * @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 + */ + 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..d7e58d502 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h @@ -0,0 +1,109 @@ +/* + * 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 + +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 + * + * 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 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. + * + * 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 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::loadSolverOptionsFromROS(ros::NodeHandle(nh, "solver_options"), solver_options); + fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout); + } +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_WINDOWED_OPTIMIZER_PARAMS_H 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 a91850825..8b709b6ae 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -33,603 +33,84 @@ */ #include -#include #include +#include #include #include -#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 { - -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()) + 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) { - 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 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) { - // 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]() + void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction &new_transaction) { - 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, 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; - } - // 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"); - } - } + std::lock_guard lock(mutex_); + timestamp_tracking_.addNewTransaction(new_transaction); } -} -void FixedLagSmoother::optimizerTimerCallback(const ros::TimerEvent& event) -{ - // If an "ignition" transaction hasn't been received, then we can't do anything yet. - if (!started_) + std::vector FixedLagSmoother::computeVariablesToMarginalize() { - 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(); - } -} + // Find the most recent variable timestamp, then carefully subtract the lag duration. + // ROS Time objects do not handle negative values. + auto start_time = getStartTime(); -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()) - { - return; + 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; } - // 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; - } + std::lock_guard lock(mutex_); + 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()) + bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction) { - auto& element = *transaction_riter; - const auto& min_stamp = element.minStamp(); - 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 " << 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; - } + 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; } -} -bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - // Tell all the plugins to stop - stopPlugins(); - // Reset the optimizer state + void FixedLagSmoother::onReset() { - 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(); + std::lock_guard lock(mutex_); 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..0a51a4645 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -34,10 +34,10 @@ #include #include #include +#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 new file mode 100644 index 000000000..545a1ae8f --- /dev/null +++ b/fuse_optimizers/src/fixed_size_smoother.cpp @@ -0,0 +1,120 @@ +/* + * 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 ((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_[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_[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."); + return false; + } + return true; +} + +void FixedSizeSmoother::onReset() +{ + std::lock_guard lock(mutex_); + timestamp_tracking_.clear(); +} + +} // namespace fuse_optimizers 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..d6360e2cb --- /dev/null +++ b/fuse_optimizers/src/fixed_size_smoother_node.cpp @@ -0,0 +1,50 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, 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; +} 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 d3127b5da..65312cf58 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -45,20 +45,22 @@ namespace fuse_optimizers { -ros::Time VariableStampIndex::currentStamp() const +ros::Time VariableStampIndex::operator[](int i) { - auto compare_stamps = [](const StampedMap::value_type& lhs, const StampedMap::value_type& rhs) + if (unique_stamps_.empty() || i < 0) { - return lhs.second < rhs.second; - }; - auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps); - if (iter != stamped_index_.end()) + return ros::Time(0, 0); + } + else if ((size_t)i < unique_stamps_.size()) { - return iter->second; + uint64_t stamp_id = (*std::next(unique_stamps_.begin(), i)).first; + ros::Time stamp; + stamp.fromNSec(stamp_id); + return stamp; } else { - return ros::Time(0, 0); + throw std::runtime_error("Index exceeds size of Variable Stamp Index."); } } @@ -98,7 +100,17 @@ 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(); + 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 } @@ -120,6 +132,20 @@ 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].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); variables_.erase(variable_uuid); } diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp new file mode 100644 index 000000000..6edc795a6 --- /dev/null +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -0,0 +1,609 @@ +/* + * 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::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(); + + // 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); + + // 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() + { + // 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; + 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; + } + 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. + { + 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; + } + + 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) + { + 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_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 + { + // 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 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 5003d3646..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,105 @@ 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) +{ + // Create an empty index + auto index = fuse_optimizers::VariableStampIndex(); + + // Verify the current stamp is 0 + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); + + // 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[index.numStates() - 1]); + + // 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[0]); +} + +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[index.numStates() - 1]); + + // 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[index.numStates() - 1]); + + // 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(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[index.numStates() - 1]); + + // 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[index.numStates() - 1]); + + // 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[i]); + } } TEST(VariableStampIndex, Query) @@ -318,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)); @@ -371,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)); @@ -379,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(); 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