diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 5e5768ad3..1edea2c38 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") - set(build_depends diagnostic_updater fuse_constraints @@ -14,6 +12,8 @@ set(build_depends std_srvs ) +find_package(OpenMP) + find_package(catkin REQUIRED COMPONENTS ${build_depends} ) @@ -51,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 diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h index 1bab2c5a5..692d151d0 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h @@ -51,10 +51,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief A simple optimizer implementation that uses batch optimization * @@ -107,10 +105,8 @@ class BatchOptimizer : public Optimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - BatchOptimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + BatchOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Destructor @@ -126,11 +122,10 @@ class BatchOptimizer : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - TransactionQueueElement( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) : - sensor_name(sensor_name), - transaction(std::move(transaction)) {} + TransactionQueueElement(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) + : sensor_name(sensor_name), transaction(std::move(transaction)) + { + } }; /** @@ -145,19 +140,19 @@ class BatchOptimizer : public Optimizer fuse_core::Transaction::SharedPtr combined_transaction_; //!< Transaction used aggregate constraints and variables //!< from multiple sensors and motions models before being //!< applied to the graph. - std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction across different threads - ParameterType params_; //!< Configuration settings for this optimizer + std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction across different threads + ParameterType params_; //!< Configuration settings for this optimizer std::atomic optimization_request_; //!< Flag to trigger a new optimization std::condition_variable optimization_requested_; //!< Condition variable used by the optimization thread to wait //!< until a new optimization is requested by the main thread - std::mutex optimization_requested_mutex_; //!< Required condition variable mutex - std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process - ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency + std::mutex optimization_requested_mutex_; //!< Required condition variable mutex + std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process + ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency TransactionQueue pending_transactions_; //!< The set of received transactions that have not been added to the //!< optimizer yet. Transactions are added by the main thread, and removed //!< and processed by the optimization thread. std::mutex pending_transactions_mutex_; //!< Synchronize modification of the pending_transactions_ container - ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction + ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction bool started_; //!< Flag indicating the optimizer is ready/has received a transaction from an ignition sensor /** @@ -200,9 +195,7 @@ class BatchOptimizer : public Optimizer * to generate connected constraints. * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin */ - void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** * @brief Update and publish diagnotics diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h index cc92ee4c8..8dc632ef0 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h @@ -45,10 +45,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class */ @@ -62,7 +60,7 @@ struct BatchOptimizerParams * may be specified in either the "optimization_period" parameter in seconds, or in the "optimization_frequency" * parameter in Hz. */ - ros::Duration optimization_period { 0.1 }; + ros::Duration optimization_period{ 0.1 }; /** * @brief The maximum time to wait for motion models to be generated for a received transaction. @@ -70,7 +68,7 @@ struct BatchOptimizerParams * Transactions are processed sequentially, so no new transactions will be added to the graph while waiting for * motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue. */ - ros::Duration transaction_timeout { 0.1 }; + ros::Duration transaction_timeout{ 0.1 }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index 6f3e083f6..3df84e8e1 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -113,11 +113,9 @@ class FixedLagSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Constructor @@ -129,18 +127,16 @@ class FixedLagSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - explicit FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + explicit FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); protected: // Read-only after construction ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-lag smoother // Guarded by mutex_ - std::mutex mutex_; //!< Mutex held while the fixed-lag smoother variables are modified - ros::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window + std::mutex mutex_; //!< Mutex held while the fixed-lag smoother variables are modified + ros::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable /** diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h index da866f292..4e78734d1 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h @@ -59,7 +59,7 @@ struct FixedLagSmootherParams : public WindowedOptimizerParams /** * @brief The duration of the smoothing window in seconds */ - ros::Duration lag_duration { 5.0 }; + ros::Duration lag_duration{ 5.0 }; /** * @brief Method for loading parameter values from ROS. diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h index 3cbe6d041..7498dff19 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -50,17 +50,17 @@ namespace fuse_optimizers { /** - * @brief A fixed-Size smoother implementation that marginalizes out variables that are older than a defined Size time + * @brief A fixed-2ize smoother implementation that marginalizes out variables that are older than a defined buffer size * * This implementation assumes that all added variable types are either derived from the fuse_variables::Stamped class, * or are directly connected to at least one fuse_variables::Stamped variable via a constraint. The current time of - * the fixed-Size smoother is determined by the newest stamp of all added fuse_variables::Stamped variables. + * the fixed-size smoother is determined by the newest stamp of all added fuse_variables::Stamped variables. * * During optimization: * (1) new variables and constraints are added to the graph * (2) the augmented graph is optimized and the variable values are updated * (3) all motion models, sensors, and publishers are notified of the updated graph - * (4) all variables older than "current time - Size duration" are marginalized out. + * (4) all variables outside of the fixed buffer are marginalized out * * Optimization is performed at a fixed frequency, controlled by the \p optimization_frequency parameter. Received * sensor transactions are queued while the optimization is processing, then applied to the graph at the start of the @@ -69,7 +69,7 @@ namespace fuse_optimizers * completion, and the next optimization will not begin until the next scheduled optimization period. * * Parameters: - * - Size_duration (float, default: 5.0) The duration of the smoothing window in seconds + * - num_states (float, default: 10) The number of unique timestamped states in the window * - motion_models (struct array) The set of motion model plugins to load * @code{.yaml} * - name: string (A unique name for this motion model) @@ -113,11 +113,9 @@ class FixedSizeSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Constructor @@ -129,17 +127,15 @@ class FixedSizeSmoother : public WindowedOptimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - explicit FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + explicit FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); protected: // Read-only after construction ParameterType::SharedPtr params_; //!< Configuration settings for this fixed-Size smoother // Guarded by mutex_ - std::mutex mutex_; //!< Mutex held while the fixed-size smoother variables are modified + std::mutex mutex_; //!< Mutex held while the fixed-size smoother variables are modified VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable /** diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h index b8e208ede..fb2c795a5 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_size_smoother_params.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,18 +34,10 @@ #ifndef FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H #define FUSE_OPTIMIZERS_FIXED_SIZE_SMOOTHER_PARAMS_H -#include #include #include -#include #include -#include - -#include -#include -#include - namespace fuse_optimizers { /** @@ -57,9 +49,9 @@ struct FixedSizeSmootherParams : public WindowedOptimizerParams SMART_PTR_DEFINITIONS(FixedSizeSmootherParams); /** - * @brief The duration of the smoothing window in seconds + * @brief Thenumber of unique stamps in the window */ - int num_states {10}; + int num_states{ 10 }; /** * @brief Method for loading parameter values from ROS. diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index dadfc76a2..1dcf3d4bd 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -49,10 +49,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief A base class that can be used to build fuse optimizer nodes * @@ -103,10 +101,8 @@ class Optimizer * @param[in] node_handle A node handle in the global namespace * @param[in] private_node_handle A node handle in the node's private namespace */ - Optimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + Optimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Destructor @@ -144,25 +140,25 @@ class Optimizer using SensorModels = std::unordered_map; // Some internal book-keeping data structures - using MotionModelGroup = std::vector; //!< A set of motion model names + using MotionModelGroup = std::vector; //!< A set of motion model names using AssociatedMotionModels = std::unordered_map; //!< sensor -> motion models group AssociatedMotionModels associated_motion_models_; //!< Tracks what motion models should be used for each sensor - fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and constraints + fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and constraints // Ordering ROS objects with callbacks last - ros::NodeHandle node_handle_; //!< Node handle in the public namespace for subscribing and advertising + ros::NodeHandle node_handle_; //!< Node handle in the public namespace for subscribing and advertising ros::NodeHandle private_node_handle_; //!< Node handle in the private namespace for reading configuration settings pluginlib::ClassLoader motion_model_loader_; //!< Pluginlib class loader for MotionModels - MotionModels motion_models_; //!< The set of motion models, addressable by name + MotionModels motion_models_; //!< The set of motion models, addressable by name pluginlib::ClassLoader publisher_loader_; //!< Pluginlib class loader for Publishers Publishers publishers_; //!< The set of publishers to execute after every graph optimization pluginlib::ClassLoader sensor_model_loader_; //!< Pluginlib class loader for SensorModels SensorModels sensor_models_; //!< The set of sensor models, addressable by name diagnostic_updater::Updater diagnostic_updater_; //!< Diagnostic updater - ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer - double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds + ros::Timer diagnostic_updater_timer_; //!< Diagnostic updater timer + double diagnostic_updater_timer_period_{ 1.0 }; //!< Diagnostic updater timer period in seconds /** * @brief Callback fired every time a SensorModel plugin creates a new transaction @@ -172,9 +168,7 @@ class Optimizer * to generate connected constraints. * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin */ - virtual void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) = 0; + virtual void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0; /** * @brief Configure the motion model plugins specified on the parameter server @@ -211,9 +205,7 @@ class Optimizer * models * @return Flag indicating if all motion model constraints were successfully generated */ - bool applyMotionModels( - const std::string& sensor_name, - fuse_core::Transaction& transaction) const; + bool applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const; /** * @brief Send the sensors, motion models, and publishers updated graph information @@ -221,19 +213,15 @@ class Optimizer * @param[in] transaction A read-only pointer to a transaction containing all recent additions and removals * @param[in] graph A read-only pointer to the graph object */ - void notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph); + void notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph); - /** - * @brief Inject a transaction callback function into the global callback queue - * - * @param[in] sensor_name The name of the sensor that produced the Transaction - * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin - */ - void injectCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction); + /** + * @brief Inject a transaction callback function into the global callback queue + * + * @param[in] sensor_name The name of the sensor that produced the Transaction + * @param[in] transaction The populated Transaction object created by the loaded SensorModel plugin + */ + void injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction); /** * @brief Clear all of the callbacks inserted into the callback queue by the injectCallback() method diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h index 59f5a186d..cb977921f 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h @@ -71,17 +71,26 @@ class VariableStampIndex /** * @brief Return true if no variables exist in the index */ - bool empty() const { return variables_.empty() && constraints_.empty(); } + bool empty() const + { + return variables_.empty() && constraints_.empty(); + } /** * @brief Returns the number of variables in the index */ - size_t size() const { return variables_.size(); } + size_t size() const + { + return variables_.size(); + } /** * @brief Returns the number of stamped robot states */ - size_t numStates() const { return unique_stamps_.size(); } + int numStates() const + { + return unique_stamps_.size(); + } /** * @brief Clear all tracked state @@ -95,19 +104,11 @@ class VariableStampIndex } /** - * @brief Returns the most recent timestamp associated with any variable - */ - ros::Time currentStamp() const; - - /** - * @brief Returns the oldest/first stamp - */ - ros::Time firstStamp() const; - - /** - * @brief Returns the ith timestamp + * @brief Provides a random access operator the the unique timestamps + * + * @param[in] i index to access, if negative Time(0) is returned */ - ros::Time ithStamp(size_t idx) const; + ros::Time operator[](int i); /** * @brief Update the index with the information from the added transactions @@ -192,7 +193,8 @@ class VariableStampIndex using ConstraintToVariablesMap = std::unordered_map>; ConstraintToVariablesMap constraints_; - std::set unique_stamps_; //!< This hold all the unique stamps to track total number of robot "states" + using StampToVariablesMap = std::map>; + StampToVariablesMap unique_stamps_; //!< This holds all the unique stamps to track total number of robot "states" /** * @brief Update this VariableStampIndex with the added constraints from the provided transaction diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h index 50332bdd6..6c4598204 100644 --- a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h @@ -112,11 +112,9 @@ class WindowedOptimizer : public Optimizer * @param[in] node_handle - A node handle in the global namespace * @param[in] private_node_handle - A node handle in the node's private namespace */ - WindowedOptimizer( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); + WindowedOptimizer(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle = ros::NodeHandle(), + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")); /** * @brief Destructor @@ -132,9 +130,18 @@ class WindowedOptimizer : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - const ros::Time& stamp() const { return transaction->stamp(); } - const ros::Time& minStamp() const { return transaction->minStamp(); } - const ros::Time& maxStamp() const { return transaction->maxStamp(); } + const ros::Time& stamp() const + { + return transaction->stamp(); + } + const ros::Time& minStamp() const + { + return transaction->minStamp(); + } + const ros::Time& maxStamp() const + { + return transaction->maxStamp(); + } }; /** @@ -174,18 +181,18 @@ class WindowedOptimizer : public Optimizer // Guarded by optimization_requested_mutex_ std::mutex optimization_requested_mutex_; //!< Required condition variable mutex ros::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers a warning if exceeded. - bool optimization_request_; //!< Flag to trigger a new optimization + bool optimization_request_; //!< Flag to trigger a new optimization std::condition_variable optimization_requested_; //!< Condition variable used by the optimization thread to wait //!< until a new optimization is requested by the main thread // Guarded by start_time_mutex_ mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable - ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction + ros::Time start_time_; //!< The timestamp of the first ignition sensor transaction // Ordering ROS objects with callbacks last - ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency + ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency ros::ServiceServer reset_service_server_; //!< Service that resets the optimizer to its initial state - ros::Subscriber reset_subscriber_; //!< Subscriber that resets the optimizer to its initial state + ros::Subscriber reset_subscriber_; //!< Subscriber that resets the optimizer to its initial state /** * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called @@ -283,6 +290,11 @@ class WindowedOptimizer : public Optimizer */ void processQueue(fuse_core::Transaction& transaction); + /** + * @brief Performs all logic when the client calls for a reset + */ + void reset(); + /** * @brief Message callback that resets the optimizer to its original state */ diff --git a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h index 5ad4305c4..d7e58d502 100644 --- a/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/windowed_optimizer_params.h @@ -60,12 +60,12 @@ struct WindowedOptimizerParams * may be specified in either the "optimization_period" parameter in seconds, or in the "optimization_frequency" * parameter in Hz. */ - ros::Duration optimization_period { 0.1 }; + ros::Duration optimization_period{ 0.1 }; /** * @brief The topic name of the advertised reset service */ - std::string reset_service { "~reset" }; + std::string reset_service{ "~reset" }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. @@ -78,7 +78,7 @@ struct WindowedOptimizerParams * Transactions are processed sequentially, so no new transactions will be added to the graph while waiting for * motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue. */ - ros::Duration transaction_timeout { 0.1 }; + ros::Duration transaction_timeout{ 0.1 }; /** * @brief Method for loading parameter values from ROS. @@ -90,7 +90,7 @@ struct WindowedOptimizerParams // Read settings from the parameter server if (nh.hasParam("optimization_frequency")) { - double optimization_frequency { 1.0 / optimization_period.toSec() }; + double optimization_frequency{ 1.0 / optimization_period.toSec() }; fuse_core::getPositiveParam(nh, "optimization_frequency", optimization_frequency); optimization_period.fromSec(1.0 / optimization_frequency); } diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index fb39aea39..35115d7b1 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -42,27 +42,21 @@ #include #include - namespace fuse_optimizers { - -BatchOptimizer::BatchOptimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), - combined_transaction_(fuse_core::Transaction::make_shared()), - optimization_request_(false), - start_time_(ros::TIME_MAX), - started_(false) +BatchOptimizer::BatchOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) + : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) + , combined_transaction_(fuse_core::Transaction::make_shared()) + , optimization_request_(false) + , start_time_(ros::TIME_MAX) + , started_(false) { params_.loadFromROS(private_node_handle); // Configure a timer to trigger optimizations - optimize_timer_ = node_handle_.createTimer( - ros::Duration(params_.optimization_period), - &BatchOptimizer::optimizerTimerCallback, - this); + optimize_timer_ = node_handle_.createTimer(ros::Duration(params_.optimization_period), + &BatchOptimizer::optimizerTimerCallback, this); // Start the optimization thread optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this); @@ -99,10 +93,11 @@ void BatchOptimizer::applyMotionModelsToQueue() if (element.transaction->stamp() + params_.transaction_timeout < current_time) { // Warn that this transaction has expired, then skip it. - ROS_ERROR_STREAM("The queued transaction with timestamp " << element.transaction->stamp() - << " could not be processed after " << (current_time - element.transaction->stamp()) - << " seconds, which is greater than the 'transaction_timeout' value of " - << params_.transaction_timeout << ". Ignoring this transaction."); + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.transaction->stamp() << " could not be processed after " + << (current_time - element.transaction->stamp()) + << " seconds, which is greater than the 'transaction_timeout' value of " + << params_.transaction_timeout << ". Ignoring this transaction."); pending_transactions_.erase(pending_transactions_.begin()); continue; } @@ -130,7 +125,7 @@ void BatchOptimizer::optimizationLoop() // Wait for the next signal to start the next optimization cycle { std::unique_lock lock(optimization_requested_mutex_); - optimization_requested_.wait(lock, [this]{ return optimization_request_ || !ros::ok(); }); // NOLINT + optimization_requested_.wait(lock, [this] { return optimization_request_ || !ros::ok(); }); // NOLINT } // If a shutdown is requested, exit now. if (!ros::ok()) @@ -180,9 +175,7 @@ void BatchOptimizer::optimizerTimerCallback(const ros::TimerEvent& /*event*/) } } -void BatchOptimizer::transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void BatchOptimizer::transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // Add the new transaction to the pending set // Either we haven't "started" yet and we want to keep a short history of transactions around diff --git a/fuse_optimizers/src/batch_optimizer_node.cpp b/fuse_optimizers/src/batch_optimizer_node.cpp index 1cd84b749..cc1dcaeda 100644 --- a/fuse_optimizers/src/batch_optimizer_node.cpp +++ b/fuse_optimizers/src/batch_optimizer_node.cpp @@ -35,8 +35,7 @@ #include #include - -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "batch_optimizer_node"); fuse_optimizers::BatchOptimizer optimizer(fuse_graphs::HashGraph::make_unique()); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 6593decc3..305c47aba 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -50,25 +50,17 @@ namespace fuse_optimizers { -FixedLagSmoother::FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), - params_(params) +FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) + : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), params_(params) { } -FixedLagSmoother::FixedLagSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - FixedLagSmoother::FixedLagSmoother( - std::move(graph), - ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), - node_handle, - private_node_handle) +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) { } @@ -85,7 +77,7 @@ std::vector FixedLagSmoother::computeVariablesToMarginalize() auto start_time = getStartTime(); std::lock_guard lock(mutex_); - auto now = timestamp_tracking_.currentStamp(); + 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)); @@ -104,11 +96,11 @@ bool FixedLagSmoother::validateTransaction(const std::string& sensor_name, const std::lock_guard lock(mutex_); if (min_stamp < lag_expiration_) { - ROS_DEBUG_STREAM( - "The current lag expiration time is " - << lag_expiration_ << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " - << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is " - << (lag_expiration_ - min_stamp) << " seconds too old. Ignoring this transaction."); + 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; diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 942423d2e..0a51a4645 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -37,8 +37,7 @@ #include #include - -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "fixed_lag_smoother_node"); ros::NodeHandle private_node_handle("~"); diff --git a/fuse_optimizers/src/fixed_size_smoother.cpp b/fuse_optimizers/src/fixed_size_smoother.cpp index 4fea8b83d..545a1ae8f 100644 --- a/fuse_optimizers/src/fixed_size_smoother.cpp +++ b/fuse_optimizers/src/fixed_size_smoother.cpp @@ -50,25 +50,17 @@ namespace fuse_optimizers { -FixedSizeSmoother::FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), - params_(params) +FixedSizeSmoother::FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) + : fuse_optimizers::WindowedOptimizer(std::move(graph), params, node_handle, private_node_handle), params_(params) { } -FixedSizeSmoother::FixedSizeSmoother( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - FixedSizeSmoother::FixedSizeSmoother( - std::move(graph), - ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), - node_handle, - private_node_handle) +FixedSizeSmoother::FixedSizeSmoother(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) + : FixedSizeSmoother::FixedSizeSmoother( + std::move(graph), ParameterType::make_shared(fuse_core::loadFromROS(private_node_handle)), + node_handle, private_node_handle) { } @@ -86,9 +78,10 @@ std::vector FixedSizeSmoother::computeVariablesToMarginalize() // if the total number of states is greater than our optimization window, then find the new state // given we remove the first n states to bring the number of states back to our desired window size - if((int)timestamp_tracking_.numStates() > params_->num_states){ + if ((int)timestamp_tracking_.numStates() > params_->num_states) + { size_t num_states_to_marginalize = timestamp_tracking_.numStates() - params_->num_states; - ros::Time new_start_time = timestamp_tracking_.ithStamp(num_states_to_marginalize - 1); + ros::Time new_start_time = timestamp_tracking_[num_states_to_marginalize - 1]; timestamp_tracking_.query(new_start_time, std::back_inserter(marginalize_variable_uuids)); } @@ -105,14 +98,14 @@ bool FixedSizeSmoother::validateTransaction(const std::string& sensor_name, cons { auto min_stamp = transaction.minStamp(); std::lock_guard lock(mutex_); - ros::Time window_start = timestamp_tracking_.firstStamp(); + ros::Time window_start = timestamp_tracking_[0]; if (min_stamp < window_start) { - ROS_DEBUG_STREAM( - "The current optimization window starts at " - << window_start << ". The queued transaction with timestamp " << transaction.stamp() << " from sensor " - << sensor_name << " has a minimum involved timestamp of " << min_stamp << ", which is prior to the beginning " - << "of this window, ignoring this transaction."); + ROS_DEBUG_STREAM("The current optimization window starts at " + << window_start << ". The queued transaction with timestamp " << transaction.stamp() + << " from sensor " << sensor_name << " has a minimum involved timestamp of " << min_stamp + << ", which is prior to the beginning " + << "of this window, ignoring this transaction."); return false; } return true; diff --git a/fuse_optimizers/src/fixed_size_smoother_node.cpp b/fuse_optimizers/src/fixed_size_smoother_node.cpp index 2e7c65ce3..d6360e2cb 100644 --- a/fuse_optimizers/src/fixed_size_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_size_smoother_node.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2019, Locus Robotics + * Copyright (c) 2022, Locus Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,8 +37,7 @@ #include #include - -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "fixed_size_smoother_node"); ros::NodeHandle private_node_handle("~"); diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 203ee1769..97ebfaa4a 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -49,10 +49,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief Plugin name and type configuration, as received from the parameter server * @@ -79,17 +77,15 @@ struct PluginConfig XmlRpc::XmlRpcValue config; //!< The entire configuration, that might have additional optional parameters }; -Optimizer::Optimizer( - fuse_core::Graph::UniquePtr graph, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - graph_(std::move(graph)), - node_handle_(node_handle), - private_node_handle_(private_node_handle), - motion_model_loader_("fuse_core", "fuse_core::MotionModel"), - publisher_loader_("fuse_core", "fuse_core::Publisher"), - sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), - diagnostic_updater_(node_handle_) +Optimizer::Optimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, + const ros::NodeHandle& private_node_handle) + : graph_(std::move(graph)) + , node_handle_(node_handle) + , private_node_handle_(private_node_handle) + , motion_model_loader_("fuse_core", "fuse_core::MotionModel") + , publisher_loader_("fuse_core", "fuse_core::Publisher") + , sensor_model_loader_("fuse_core", "fuse_core::SensorModel") + , diagnostic_updater_(node_handle_) { // Setup diagnostics updater private_node_handle_.param("diagnostic_updater_timer_period", diagnostic_updater_timer_period_, @@ -138,12 +134,12 @@ void Optimizer::loadMotionModels() { // Validate the parameter server values const auto& motion_model = motion_models[motion_model_index]; - if ( (motion_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!motion_model.hasMember("name")) - || (!motion_model.hasMember("type"))) + if ((motion_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!motion_model.hasMember("name")) || + (!motion_model.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " - "-{name: string, type: string}"); + throw std::invalid_argument( + "The 'motion_models' parameter should be a list of the form: " + "-{name: string, type: string}"); } motion_model_configs.emplace_back(static_cast(motion_model["name"]), @@ -156,11 +152,12 @@ void Optimizer::loadMotionModels() for (const auto& motion_model : motion_models) { const auto& motion_model_config = motion_model.second; - if ( (motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!motion_model_config.hasMember("type"))) + if ((motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || + (!motion_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a struct of the form: " - "{string: {type: string}}"); + throw std::invalid_argument( + "The 'motion_models' parameter should be a struct of the form: " + "{string: {type: string}}"); } motion_model_configs.emplace_back(static_cast(motion_model.first), @@ -169,8 +166,9 @@ void Optimizer::loadMotionModels() } else { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " - "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); + throw std::invalid_argument( + "The 'motion_models' parameter should be a list of the form: " + "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } for (const auto& config : motion_model_configs) @@ -204,12 +202,12 @@ void Optimizer::loadSensorModels() { // Validate the parameter server values const auto& sensor_model = sensor_models[sensor_model_index]; - if ( (sensor_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!sensor_model.hasMember("name")) - || (!sensor_model.hasMember("type"))) + if ((sensor_model.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!sensor_model.hasMember("name")) || + (!sensor_model.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " - "-{name: string, type: string, motion_models: [name1, name2, ...]}"); + throw std::invalid_argument( + "The 'sensor_models' parameter should be a list of the form: " + "-{name: string, type: string, motion_models: [name1, name2, ...]}"); } sensor_model_configs.emplace_back(static_cast(sensor_model["name"]), @@ -223,11 +221,12 @@ void Optimizer::loadSensorModels() { // Validate the parameter server values const auto& sensor_model_config = sensor_model.second; - if ( (sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!sensor_model_config.hasMember("type"))) + if ((sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || + (!sensor_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a struct of the form: " - "{string: {type: string, motion_models: [name1, name2, ...]}}"); + throw std::invalid_argument( + "The 'sensor_models' parameter should be a struct of the form: " + "{string: {type: string, motion_models: [name1, name2, ...]}}"); } sensor_model_configs.emplace_back(static_cast(sensor_model.first), @@ -236,10 +235,11 @@ void Optimizer::loadSensorModels() } else { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " - "-{name: string, type: string, motion_models: [name1, name2, ...]} " - "or a struct of the form: " - "{string: {type: string, motion_models: [name1, name2, ...]}}"); + throw std::invalid_argument( + "The 'sensor_models' parameter should be a list of the form: " + "-{name: string, type: string, motion_models: [name1, name2, ...]} " + "or a struct of the form: " + "{string: {type: string, motion_models: [name1, name2, ...]}}"); } for (const auto& config : sensor_model_configs) @@ -250,16 +250,15 @@ void Optimizer::loadSensorModels() // Create a sensor object using pluginlib. This will throw if the plugin name is not found. auto sensor_model = sensor_model_loader_.createUniqueInstance(config.type); // Initialize the sensor - sensor_model->initialize( - config.name, - std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); + sensor_model->initialize(config.name, + std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); // Store the sensor in a member variable for use later sensor_models_.emplace(config.name, SensorModelInfo{ std::move(sensor_model), ignition }); // NOLINT(whitespace/braces) // Parse out the list of associated motion models, if any - if ( (config.config.hasMember("motion_models")) - && (config.config["motion_models"].getType() == XmlRpc::XmlRpcValue::TypeArray)) + if ((config.config.hasMember("motion_models")) && + (config.config["motion_models"].getType() == XmlRpc::XmlRpcValue::TypeArray)) { XmlRpc::XmlRpcValue motion_model_list = config.config["motion_models"]; for (int32_t motion_model_index = 0; motion_model_index < motion_model_list.size(); ++motion_model_index) @@ -268,9 +267,10 @@ void Optimizer::loadSensorModels() associated_motion_models_[config.name].push_back(motion_model_name); if (motion_models_.find(motion_model_name) == motion_models_.end()) { - ROS_WARN_STREAM("Sensor model '" << config.name << "' is configured to use motion model '" << - motion_model_name << "', but no motion model with that name currently exists. This is " << - "likely a configuration error."); + ROS_WARN_STREAM("Sensor model '" << config.name << "' is configured to use motion model '" + << motion_model_name + << "', but no motion model with that name currently exists. This is " + << "likely a configuration error."); } } } @@ -297,12 +297,12 @@ void Optimizer::loadPublishers() { // Validate the parameter server values const auto& publisher = publishers[publisher_index]; - if ( (publisher.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!publisher.hasMember("name")) - || (!publisher.hasMember("type"))) + if ((publisher.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher.hasMember("name")) || + (!publisher.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string}"); + throw std::invalid_argument( + "The 'publishers' parameter should be a list of the form: " + "-{name: string, type: string}"); } publisher_configs.emplace_back(static_cast(publisher["name"]), @@ -316,11 +316,11 @@ void Optimizer::loadPublishers() { // Validate the parameter server values const auto& publisher_config = publisher.second; - if ( (publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) - || (!publisher_config.hasMember("type"))) + if ((publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher_config.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a struct of the form: " - "{string: {type: string}}"); + throw std::invalid_argument( + "The 'publishers' parameter should be a struct of the form: " + "{string: {type: string}}"); } publisher_configs.emplace_back(static_cast(publisher.first), @@ -329,8 +329,9 @@ void Optimizer::loadPublishers() } else { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); + throw std::invalid_argument( + "The 'publishers' parameter should be a list of the form: " + "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } for (const auto& config : publisher_configs) @@ -346,9 +347,7 @@ void Optimizer::loadPublishers() diagnostic_updater_.force_update(); } -bool Optimizer::applyMotionModels( - const std::string& sensor_name, - fuse_core::Transaction& transaction) const +bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const { // Check for trivial cases where we don't have to do anything auto iter = associated_motion_models_.find(sensor_name); @@ -368,16 +367,15 @@ bool Optimizer::applyMotionModels( catch (const std::exception& e) { ROS_ERROR_STREAM("Error generating constraints for sensor '" << sensor_name << "' " - << "from motion model '" << motion_model_name << "'. Error: " << e.what()); + << "from motion model '" << motion_model_name + << "'. Error: " << e.what()); success = false; } } return success; } -void Optimizer::notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph) { for (const auto& name__sensor_model : sensor_models_) { @@ -387,8 +385,8 @@ void Optimizer::notify( } catch (const std::exception& e) { - ROS_ERROR_STREAM("Failed calling graphCallback() on sensor '" << name__sensor_model.first << "'. " << - "Error: " << e.what()); + ROS_ERROR_STREAM("Failed calling graphCallback() on sensor '" << name__sensor_model.first << "'. " + << "Error: " << e.what()); continue; } } @@ -400,8 +398,8 @@ void Optimizer::notify( } catch (const std::exception& e) { - ROS_ERROR_STREAM("Failed calling graphCallback() on motion model '" << name__motion_model.first << "." << - " Error: " << e.what()); + ROS_ERROR_STREAM("Failed calling graphCallback() on motion model '" << name__motion_model.first << "." + << " Error: " << e.what()); continue; } } @@ -413,24 +411,22 @@ void Optimizer::notify( } catch (const std::exception& e) { - ROS_ERROR_STREAM("Failed calling notify() on publisher '" << name__publisher.first << "." << - " Error: " << e.what()); + ROS_ERROR_STREAM("Failed calling notify() on publisher '" << name__publisher.first << "." + << " Error: " << e.what()); continue; } } } -void Optimizer::injectCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void Optimizer::injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // We are going to insert a call to the derived class's transactionCallback() method into the global callback queue. // This returns execution to the sensor's thread quickly by moving the transaction processing to the optimizer's // thread. And by using the existing ROS callback queue, we simplify the threading model of the optimizer. ros::getGlobalCallbackQueue()->addCallback( - boost::make_shared>( - std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))), - reinterpret_cast(this)); + boost::make_shared>( + std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))), + reinterpret_cast(this)); } void Optimizer::clearCallbacks() diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index d8b63bded..65312cf58 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -45,31 +45,23 @@ namespace fuse_optimizers { -ros::Time VariableStampIndex::currentStamp() const +ros::Time VariableStampIndex::operator[](int i) { - if(!unique_stamps_.empty()){ - return *unique_stamps_.end(); + if (unique_stamps_.empty() || i < 0) + { + return ros::Time(0, 0); } - return ros::Time(0, 0); -} - -ros::Time VariableStampIndex::firstStamp() const -{ - if(!unique_stamps_.empty()){ - return *unique_stamps_.begin(); + else if ((size_t)i < unique_stamps_.size()) + { + uint64_t stamp_id = (*std::next(unique_stamps_.begin(), i)).first; + ros::Time stamp; + stamp.fromNSec(stamp_id); + return stamp; } - return ros::Time(0, 0); -} - -ros::Time VariableStampIndex::ithStamp(size_t idx) const{ - if(idx < unique_stamps_.size()){ - std::set::iterator it = unique_stamps_.begin(); - for(size_t i = 0; i < idx; i++){ - it++; - } - return *(it); + else + { + throw std::runtime_error("Index exceeds size of Variable Stamp Index."); } - return ros::Time(0, 0); } void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transaction) @@ -108,10 +100,16 @@ void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& trans auto stamped_variable = dynamic_cast(&variable); if (stamped_variable) { + uint64_t stamp = stamped_variable->stamp().toNSec(); stamped_index_[variable.uuid()] = stamped_variable->stamp(); - // add to unique stamps - if(unique_stamps_.find(stamped_variable->stamp()) == unique_stamps_.end()){ - unique_stamps_.insert(stamped_variable->stamp()); + if (unique_stamps_.find(stamp) != unique_stamps_.end()) + { + unique_stamps_[stamp].insert(variable.uuid()); + } + else + { + std::unordered_set set = { variable.uuid() }; + unique_stamps_.insert({ stamp, set }); } } variables_[variable.uuid()]; // Add an empty set of constraints @@ -135,9 +133,17 @@ void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction& tra for (const auto& variable_uuid : transaction.removedVariables()) { // remove from unique stamps - auto stamp = stamped_index_[variable_uuid]; - if(unique_stamps_.find(stamp) != unique_stamps_.end()){ - unique_stamps_.erase(stamp); + auto stamp = stamped_index_[variable_uuid].toNSec(); + if (unique_stamps_.find(stamp) != unique_stamps_.end()) + { + if (unique_stamps_[stamp].find(variable_uuid) != unique_stamps_[stamp].end()) + { + unique_stamps_[stamp].erase(variable_uuid); + } + if (unique_stamps_[stamp].size() == 0) + { + unique_stamps_.erase(stamp); + } } stamped_index_.erase(variable_uuid); diff --git a/fuse_optimizers/src/windowed_optimizer.cpp b/fuse_optimizers/src/windowed_optimizer.cpp index b62c41932..184b0b745 100644 --- a/fuse_optimizers/src/windowed_optimizer.cpp +++ b/fuse_optimizers/src/windowed_optimizer.cpp @@ -58,9 +58,8 @@ namespace * @return A reverse iterator pointing to the element after the erased element */ template -typename std::vector::reverse_iterator erase( - std::vector& container, - typename std::vector::reverse_iterator position) +typename std::vector::reverse_iterator erase(std::vector& container, + typename std::vector::reverse_iterator position) { // Reverse iterators are weird // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator @@ -76,17 +75,14 @@ typename std::vector::reverse_iterator erase( namespace fuse_optimizers { -WindowedOptimizer::WindowedOptimizer( - fuse_core::Graph::UniquePtr graph, - const ParameterType::SharedPtr& params, - const ros::NodeHandle& node_handle, - const ros::NodeHandle& private_node_handle) : - fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle), - params_(params), - ignited_(false), - optimization_running_(true), - started_(false), - optimization_request_(false) +WindowedOptimizer::WindowedOptimizer(fuse_core::Graph::UniquePtr graph, const ParameterType::SharedPtr& params, + const ros::NodeHandle& node_handle, const ros::NodeHandle& private_node_handle) + : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) + , params_(params) + , ignited_(false) + , optimization_running_(true) + , started_(false) + , optimization_request_(false) { // Test for auto-start autostart(); @@ -96,19 +92,15 @@ WindowedOptimizer::WindowedOptimizer( // Configure a timer to trigger optimizations optimize_timer_ = - node_handle_.createTimer(params_->optimization_period, &WindowedOptimizer::optimizerTimerCallback, this); + node_handle_.createTimer(params_->optimization_period, &WindowedOptimizer::optimizerTimerCallback, this); // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = node_handle_.advertiseService( - ros::names::resolve(params_->reset_service), - &WindowedOptimizer::resetServiceCallback, - this); + reset_service_server_ = node_handle_.advertiseService(ros::names::resolve(params_->reset_service), + &WindowedOptimizer::resetServiceCallback, this); // Subscribe to a reset message that will reset the optimizer to the initial state - reset_subscriber_ = node_handle_.subscribe( - ros::names::resolve(params_->reset_service), 10, - &WindowedOptimizer::resetMessageCallback, - this); + reset_subscriber_ = node_handle_.subscribe(ros::names::resolve(params_->reset_service), 10, + &WindowedOptimizer::resetMessageCallback, this); } WindowedOptimizer::~WindowedOptimizer() @@ -125,10 +117,8 @@ WindowedOptimizer::~WindowedOptimizer() void WindowedOptimizer::autostart() { - if (std::none_of( - sensor_models_.begin(), - sensor_models_.end(), - [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) + if (std::none_of(sensor_models_.begin(), sensor_models_.end(), + [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) { // No ignition sensors were provided. Auto-start. started_ = true; @@ -139,8 +129,7 @@ void WindowedOptimizer::autostart() void WindowedOptimizer::optimizationLoop() { - auto exit_wait_condition = [this]() - { + auto exit_wait_condition = [this]() { return this->optimization_request_ || !this->optimization_running_ || !ros::ok(); }; // Optimize constraints until told to exit @@ -177,19 +166,25 @@ void WindowedOptimizer::optimizationLoop() } // check if new transaction has added constraints to variables that are to be marginalized std::vector faulty_constraints; - for(auto& c: new_transaction->addedConstraints()){ - for(auto var_uuid: c.variables()){ - for (auto marginal_uuid : marginal_transaction_.removedVariables()) { - if (var_uuid == marginal_uuid) { + for (auto& c : new_transaction->addedConstraints()) + { + for (auto var_uuid : c.variables()) + { + for (auto marginal_uuid : marginal_transaction_.removedVariables()) + { + if (var_uuid == marginal_uuid) + { faulty_constraints.push_back(c.uuid()); break; } } } } - if(faulty_constraints.size() > 0){ + if (faulty_constraints.size() > 0) + { ROS_WARN_STREAM("Removing invalid constraints."); - for(auto& faulty_constraint: faulty_constraints){ + for (auto& faulty_constraint : faulty_constraints) + { new_transaction->removeConstraint(faulty_constraint); } } @@ -211,11 +206,10 @@ void WindowedOptimizer::optimizationLoop() oss << "\nTransaction:\n"; new_transaction->print(oss); - ROS_FATAL_STREAM( - "Failed to update graph with transaction: " << ex.what() - << "\nLeaving optimization loop and requesting " - "node shutdown...\n" - << oss.str()); + ROS_FATAL_STREAM("Failed to update graph with transaction: " << ex.what() + << "\nLeaving optimization loop and requesting " + "node shutdown...\n" + << oss.str()); ros::requestShutdown(); break; } @@ -229,9 +223,8 @@ void WindowedOptimizer::optimizationLoop() // Abort if optimization failed. Not converging is not a failure because the solution found is usable. if (!summary_.IsSolutionUsable()) { - ROS_FATAL_STREAM( - "Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " + << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); ROS_INFO_STREAM(summary_.FullReport()); ros::requestShutdown(); break; @@ -241,7 +234,7 @@ void WindowedOptimizer::optimizationLoop() // Determination of which variables to marginalize is delegated to derived classes auto variables_to_marginalize = computeVariablesToMarginalize(); marginal_transaction_ = - fuse_constraints::marginalizeVariables(ros::this_node::getName(), variables_to_marginalize, *graph_); + fuse_constraints::marginalizeVariables(ros::this_node::getName(), variables_to_marginalize, *graph_); // Perform any post-marginal cleanup -- Delegated to derived classes postprocessMarginalization(marginal_transaction_); // Note: The marginal transaction will not be applied until the next optimization iteration @@ -249,10 +242,8 @@ void WindowedOptimizer::optimizationLoop() auto optimization_complete = ros::Time::now(); if (optimization_complete > optimization_deadline) { - ROS_WARN_STREAM_THROTTLE( - 10.0, - "Optimization exceeded the configured duration by " << (optimization_complete - optimization_deadline) - << "s"); + ROS_WARN_STREAM_THROTTLE(10.0, "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline) << "s"); } } } @@ -315,10 +306,10 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) { // We just started, but the oldest transaction is not from an ignition sensor. We will still process the // transaction, but we do not enforce it is processed individually. - ROS_ERROR_STREAM( - "The queued transaction with timestamp " << element.stamp() << " from sensor " << element.sensor_name - << " is not an ignition sensor transaction. " - << "This transaction will not be processed individually."); + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.stamp() << " from sensor " << element.sensor_name + << " is not an ignition sensor transaction. " + << "This transaction will not be processed individually."); } else { @@ -333,21 +324,20 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) { // The motion model processing failed. When this happens to an ignition sensor transaction there is no point on // trying again next time, so we ignore this transaction. - ROS_ERROR_STREAM( - "The queued ignition transaction with timestamp " << element.stamp() << " from sensor " << element.sensor_name - << " could not be processed. Ignoring this ignition " - "transaction."); + ROS_ERROR_STREAM("The queued ignition transaction with timestamp " + << element.stamp() << " from sensor " << element.sensor_name + << " could not be processed. Ignoring this ignition " + "transaction."); // Remove the ignition transaction that just failed and purge all transactions after it. But if we find another // ignition transaction, we schedule it to be processed in the next optimization cycle. erase(pending_transactions_, transaction_rbegin); - const auto pending_ignition_transaction_iter = std::find_if( - pending_transactions_.rbegin(), - pending_transactions_.rend(), - [this](const auto& element) { // NOLINT(whitespace/braces) - return sensor_models_.at(element.sensor_name).ignition; - }); // NOLINT(whitespace/braces) + const auto pending_ignition_transaction_iter = + std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), + [this](const auto& element) { // NOLINT(whitespace/braces) + return sensor_models_.at(element.sensor_name).ignition; + }); // NOLINT(whitespace/braces) if (pending_ignition_transaction_iter == pending_transactions_.rend()) { // There is no other ignition transaction pending. We simply roll back to not started state and all other @@ -401,15 +391,13 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) if (max_stamp + params_->transaction_timeout < current_time) { // Warn that this transaction has expired, then skip it. - ROS_ERROR_STREAM( - "The queued transaction with timestamp " << element.stamp() - << " and maximum " - "involved stamp of " - << max_stamp << " from sensor " << element.sensor_name - << " could not be processed after " << (current_time - max_stamp) - << " seconds, " - "which is greater than the 'transaction_timeout' value of " - << params_->transaction_timeout << ". Ignoring this transaction."); + ROS_ERROR_STREAM("The queued transaction with timestamp " + << element.stamp() << " and maximum " + "involved stamp of " + << max_stamp << " from sensor " << element.sensor_name << " could not be processed after " + << (current_time - max_stamp) << " seconds, " + "which is greater than the 'transaction_timeout' value of " + << params_->transaction_timeout << ". Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); } else @@ -422,7 +410,8 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction) } } -void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&){ +void WindowedOptimizer::reset() +{ // Tell all the plugins to stop stopPlugins(); // Reset the optimizer state @@ -456,56 +445,32 @@ void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&){ return; } -bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&) { - // Tell all the plugins to stop - stopPlugins(); - // Reset the optimizer state - { - std::lock_guard lock(optimization_requested_mutex_); - optimization_request_ = false; - } - started_ = false; - ignited_ = false; - setStartTime(ros::Time(0, 0)); - // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the - // pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to - // prevent the possibility of deadlocks. - { - std::lock_guard lock(optimization_mutex_); - // Clear all pending transactions - { - std::lock_guard lock(pending_transactions_mutex_); - pending_transactions_.clear(); - } - // Clear the graph and marginal tracking states - graph_->clear(); - marginal_transaction_ = fuse_core::Transaction(); - } - // Perform any required reset operations for derived classes - onReset(); - // Tell all the plugins to start - startPlugins(); - // Test for auto-start - autostart(); + // perform reset logic + reset(); + return; +} +bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + // perform reset logic + reset(); return true; } -void WindowedOptimizer::transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void WindowedOptimizer::transactionCallback(const std::string& sensor_name, + fuse_core::Transaction::SharedPtr transaction) { // If this transaction occurs before the start time, just ignore it auto start_time = getStartTime(); const auto max_time = transaction->maxStamp(); if (started_ && max_time < start_time) { - ROS_DEBUG_STREAM( - "Received a transaction before the start time from sensor '" - << sensor_name << "'.\n" - << " start_time: " << start_time << ", maximum involved stamp: " << max_time - << ", difference: " << (start_time - max_time) << "s"); + ROS_DEBUG_STREAM("Received a transaction before the start time from sensor '" + << sensor_name << "'.\n" + << " start_time: " << start_time << ", maximum involved stamp: " << max_time + << ", difference: " << (start_time - max_time) << "s"); return; } { @@ -514,12 +479,11 @@ void WindowedOptimizer::transactionCallback( // Add the new transaction to the pending set // The pending set is arranged "smallest stamp last" to making popping off the back more efficient - auto comparator = [](const ros::Time& value, const TransactionQueueElement& element) - { + auto comparator = [](const ros::Time& value, const TransactionQueueElement& element) { return value >= element.stamp(); }; auto position = - std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); + std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT // If we haven't "started" yet.. @@ -540,16 +504,13 @@ void WindowedOptimizer::transactionCallback( // TODO(efernandez) Do '&min_time = std::as_const(start_time)' when C++17 is supported and we can use // std::as_const: https://en.cppreference.com/w/cpp/utility/as_const pending_transactions_.erase( - std::remove_if( - pending_transactions_.begin(), - pending_transactions_.end(), - [&sensor_name, - max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); + std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), + [&sensor_name, max_time, + &min_time = start_time ](const auto& transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); } else { diff --git a/fuse_optimizers/test/example_optimizer.h b/fuse_optimizers/test/example_optimizer.h index 1464b2615..7708f5720 100644 --- a/fuse_optimizers/test/example_optimizer.h +++ b/fuse_optimizers/test/example_optimizer.h @@ -39,7 +39,6 @@ #include #include - /** * @brief Example optimizer that exposes the motion and sensor models, and the publishers, so we can check the expected * ones are loaded. @@ -50,7 +49,7 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer SMART_PTR_DEFINITIONS(ExampleOptimizer); ExampleOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), - const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")) + const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")) : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) { } @@ -70,9 +69,7 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer return publishers_; } - void transactionCallback( - const std::string& sensor_name, - fuse_core::Transaction::SharedPtr transaction) override + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override { } }; diff --git a/fuse_optimizers/test/test_fixed_lag_ignition.cpp b/fuse_optimizers/test/test_fixed_lag_ignition.cpp index a18571028..705d64a54 100644 --- a/fuse_optimizers/test/test_fixed_lag_ignition.cpp +++ b/fuse_optimizers/test/test_fixed_lag_ignition.cpp @@ -38,7 +38,6 @@ #include - TEST(FixedLagIgnition, SetInitialState) { // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify. @@ -72,8 +71,7 @@ TEST(FixedLagIgnition, SetInitialState) // The 'set_pose' service call triggers all of the sensors to resubscribe to their topics. // I need to wait for those subscribers to be ready before sending them sensor data. ros::WallTime subscriber_timeout = ros::WallTime::now() + ros::WallDuration(1.0); - while ((relative_pose_publisher.getNumSubscribers() < 1u) && - (ros::WallTime::now() < subscriber_timeout)) + while ((relative_pose_publisher.getNumSubscribers() < 1u) && (ros::WallTime::now() < subscriber_timeout)) { ros::WallDuration(0.01).sleep(); } @@ -113,8 +111,7 @@ TEST(FixedLagIgnition, SetInitialState) // Wait for the optimizer to process all queued transactions ros::Time result_timeout = ros::Time::now() + ros::Duration(3.0); auto odom_msg = nav_msgs::Odometry::ConstPtr(); - while ((!odom_msg || odom_msg->header.stamp != ros::Time(3, 0)) && - (ros::Time::now() < result_timeout)) + while ((!odom_msg || odom_msg->header.stamp != ros::Time(3, 0)) && (ros::Time::now() < result_timeout)) { odom_msg = ros::topic::waitForMessage("/odom", ros::Duration(1.0)); } diff --git a/fuse_optimizers/test/test_optimizer.cpp b/fuse_optimizers/test/test_optimizer.cpp index 804e82bba..f1d075de6 100644 --- a/fuse_optimizers/test/test_optimizer.cpp +++ b/fuse_optimizers/test/test_optimizer.cpp @@ -42,7 +42,6 @@ #include #include - TEST(Optimizer, Constructor) { // Create optimizer: @@ -63,12 +62,12 @@ TEST(Optimizer, Constructor) "unicycle_2d_ignition", "wheel_odometry" }; const std::vector expected_publishers = { "odometry_publisher", "serialized_publisher" }; - ASSERT_TRUE(std::is_sorted(expected_motion_models.begin(), expected_motion_models.end())) - << expected_motion_models << " is not sorted."; - ASSERT_TRUE(std::is_sorted(expected_sensor_models.begin(), expected_sensor_models.end())) - << expected_sensor_models << " is not sorted."; - ASSERT_TRUE(std::is_sorted(expected_publishers.begin(), expected_publishers.end())) - << expected_publishers << " is not sorted."; + ASSERT_TRUE(std::is_sorted(expected_motion_models.begin(), expected_motion_models.end())) << expected_motion_models + << " is not sorted."; + ASSERT_TRUE(std::is_sorted(expected_sensor_models.begin(), expected_sensor_models.end())) << expected_sensor_models + << " is not sorted."; + ASSERT_TRUE(std::is_sorted(expected_publishers.begin(), expected_publishers.end())) << expected_publishers + << " is not sorted."; // Compute the symmetric difference between the expected and actual motion and sensor models, and publishers: const auto difference_motion_models = set_symmetric_difference(expected_motion_models, motion_models); @@ -77,14 +76,14 @@ TEST(Optimizer, Constructor) // Check the symmetric difference is empty, i.e. the actual motion and sensor models, and publishers are the same as // the expected ones: - EXPECT_TRUE(difference_motion_models.empty()) - << "Actual: " << motion_models << "\nExpected: " << expected_motion_models - << "\nDifference: " << difference_motion_models; - EXPECT_TRUE(difference_sensor_models.empty()) - << "Actual: " << sensor_models << "\nExpected: " << expected_sensor_models - << "\nDifference: " << difference_sensor_models; - EXPECT_TRUE(difference_publishers.empty()) - << "Actual: " << publishers << "\nExpected: " << expected_publishers << "\nDifference: " << difference_publishers; + EXPECT_TRUE(difference_motion_models.empty()) << "Actual: " << motion_models + << "\nExpected: " << expected_motion_models + << "\nDifference: " << difference_motion_models; + EXPECT_TRUE(difference_sensor_models.empty()) << "Actual: " << sensor_models + << "\nExpected: " << expected_sensor_models + << "\nDifference: " << difference_sensor_models; + EXPECT_TRUE(difference_publishers.empty()) << "Actual: " << publishers << "\nExpected: " << expected_publishers + << "\nDifference: " << difference_publishers; } int main(int argc, char** argv) diff --git a/fuse_optimizers/test/test_variable_stamp_index.cpp b/fuse_optimizers/test/test_variable_stamp_index.cpp index 2fd9e8be3..4e2e739ff 100644 --- a/fuse_optimizers/test/test_variable_stamp_index.cpp +++ b/fuse_optimizers/test/test_variable_stamp_index.cpp @@ -51,7 +51,6 @@ #include #include - /** * @brief Create a simple stamped Variable for testing */ @@ -60,10 +59,8 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp public: FUSE_VARIABLE_DEFINITIONS(StampedVariable); - explicit StampedVariable(const ros::Time& stamp = ros::Time(0, 0)) : - fuse_core::Variable(fuse_core::uuid::generate()), - fuse_variables::Stamped(stamp), - data_{} + explicit StampedVariable(const ros::Time& stamp = ros::Time(0, 0)) + : fuse_core::Variable(fuse_core::uuid::generate()), fuse_variables::Stamped(stamp), data_{} { } @@ -98,12 +95,12 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template + template void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -117,9 +114,7 @@ class UnstampedVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(UnstampedVariable); - UnstampedVariable() : - fuse_core::Variable(fuse_core::uuid::generate()), - data_{} + UnstampedVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_{} { } @@ -154,11 +149,11 @@ class UnstampedVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template + template void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -174,30 +169,24 @@ class GenericConstraint : public fuse_core::Constraint GenericConstraint() = default; - GenericConstraint(const std::string& source, std::initializer_list variable_uuids) : - Constraint(source, variable_uuids) + GenericConstraint(const std::string& source, std::initializer_list variable_uuids) + : Constraint(source, variable_uuids) { } - explicit GenericConstraint(const std::string& source, const fuse_core::UUID& variable1) : - fuse_core::Constraint(source, {variable1}) + explicit GenericConstraint(const std::string& source, const fuse_core::UUID& variable1) + : fuse_core::Constraint(source, { variable1 }) { } - GenericConstraint( - const std::string& source, - const fuse_core::UUID& variable1, - const fuse_core::UUID& variable2) : - fuse_core::Constraint(source, {variable1, variable2}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + : fuse_core::Constraint(source, { variable1, variable2 }) { } - GenericConstraint( - const std::string& source, - const fuse_core::UUID& variable1, - const fuse_core::UUID& variable2, - const fuse_core::UUID& variable3) : - fuse_core::Constraint(source, {variable1, variable2, variable3}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2, + const fuse_core::UUID& variable3) + : fuse_core::Constraint(source, { variable1, variable2, variable3 }) { } @@ -220,16 +209,15 @@ class GenericConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template + template void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; BOOST_CLASS_EXPORT(GenericConstraint); - TEST(VariableStampIndex, Size) { // Create an empty index @@ -261,7 +249,7 @@ TEST(VariableStampIndex, CurrentStamp) auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -270,7 +258,7 @@ TEST(VariableStampIndex, CurrentStamp) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add a stamped variable auto x2 = StampedVariable::make_shared(ros::Time(1, 0)); @@ -279,7 +267,7 @@ TEST(VariableStampIndex, CurrentStamp) index.addNewTransaction(transaction2); // Verify the current stamp is now Time(1, 0) - EXPECT_EQ(ros::Time(1, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(1, 0), index[index.numStates() - 1]); } TEST(VariableStampIndex, FirstStamp) @@ -288,7 +276,7 @@ TEST(VariableStampIndex, FirstStamp) auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -297,7 +285,7 @@ TEST(VariableStampIndex, FirstStamp) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add two stamped variables auto x2 = StampedVariable::make_shared(ros::Time(1, 0)); @@ -311,7 +299,7 @@ TEST(VariableStampIndex, FirstStamp) index.addNewTransaction(transaction3); // Verify the current stamp is now Time(1, 0) - EXPECT_EQ(ros::Time(1, 0), index.firstStamp()); + EXPECT_EQ(ros::Time(1, 0), index[0]); } TEST(VariableStampIndex, NumSates) @@ -320,7 +308,7 @@ TEST(VariableStampIndex, NumSates) auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -329,12 +317,13 @@ TEST(VariableStampIndex, NumSates) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add N stamped variables int N = 10; - for(int i = 0; i < N; i++){ + for (int i = 0; i < N; i++) + { auto x = StampedVariable::make_shared(ros::Time(i, 0)); auto transaction = fuse_core::Transaction(); transaction.addVariable(x); @@ -342,17 +331,16 @@ TEST(VariableStampIndex, NumSates) } // Verify the number of unique stamps - EXPECT_EQ((size_t)N, index.numStates()); + EXPECT_EQ(N, index.numStates()); } - TEST(VariableStampIndex, ithStamp) { // Create an empty index auto index = fuse_optimizers::VariableStampIndex(); // Verify the current stamp is 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add an unstamped variable auto x1 = UnstampedVariable::make_shared(); @@ -361,11 +349,12 @@ TEST(VariableStampIndex, ithStamp) index.addNewTransaction(transaction1); // Verify the current stamp is still 0 - EXPECT_EQ(ros::Time(0, 0), index.currentStamp()); + EXPECT_EQ(ros::Time(0, 0), index[index.numStates() - 1]); // Add N stamped variables int N = 10; - for(int i = 0; i < N; i++){ + for (int i = 0; i < N; i++) + { auto x = StampedVariable::make_shared(ros::Time(i, 0)); auto transaction = fuse_core::Transaction(); transaction.addVariable(x); @@ -373,8 +362,9 @@ TEST(VariableStampIndex, ithStamp) } // Verify the stamps of the states using ithStamp function - for(int i = 0; i < N; i++){ - EXPECT_EQ(ros::Time(i, 0), index.ithStamp((size_t)i)); + for (int i = 0; i < N; i++) + { + EXPECT_EQ(ros::Time(i, 0), index[i]); } } @@ -414,7 +404,7 @@ TEST(VariableStampIndex, Query) index.query(ros::Time(1, 500000), std::back_inserter(actual1)); EXPECT_EQ(expected1, actual1); - auto expected2 = std::vector{x1->uuid(), l1->uuid()}; + auto expected2 = std::vector{ x1->uuid(), l1->uuid() }; std::sort(expected2.begin(), expected2.end()); auto actual2 = std::vector(); index.query(ros::Time(2, 500000), std::back_inserter(actual2)); @@ -467,7 +457,7 @@ TEST(VariableStampIndex, MarginalTransaction) EXPECT_EQ(4u, index.size()); // And the marginal constraint x3->l1 should not affect future queries - auto expected = std::vector{l1->uuid()}; + auto expected = std::vector{ l1->uuid() }; std::sort(expected.begin(), expected.end()); auto actual = std::vector(); index.query(ros::Time(2, 500000), std::back_inserter(actual)); @@ -475,7 +465,7 @@ TEST(VariableStampIndex, MarginalTransaction) EXPECT_EQ(expected, actual); } -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS();