diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index 63d52f60a..fe83ec100 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -112,6 +113,7 @@ class FixedLagSmoother : public Optimizer { public: SMART_PTR_DEFINITIONS(FixedLagSmoother); + using ParameterType = FixedLagSmootherParams; /** * @brief Constructor @@ -153,21 +155,16 @@ class FixedLagSmoother : public Optimizer */ using TransactionQueue = VectorQueue>; - std::vector ignition_sensors_; //!< The set of sensors whose transactions will trigger the optimizer - //!< thread to start running. This is designed to keep the system idle - //!< until the origin constraint has been received. - ros::Duration lag_duration_; //!< Parameter that controls the time window of the fixed lag smoother. Variables - //!< older than the lag duration will be marginalized out fuse_core::Transaction marginal_transaction_; //!< The marginals to add during the next optimization cycle ros::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers a warning if exceeded. std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized - ros::Duration optimization_period_; //!< The expected time between optimization cycles 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::atomic optimization_running_; //!< Flag indicating the optimization thread should be running std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process + ParameterType params_; //!< Configuration settings for this fixed-lag smoother 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. @@ -175,8 +172,6 @@ class FixedLagSmoother : public Optimizer 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 VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with each variable - ros::Duration transaction_timeout_; //!< Parameter that controls how long to wait for a transaction to be processed - //!< successfully before kicking it out of the queue. // Ordering ROS objects with callbacks last ros::Timer optimize_timer_; //!< Trigger an optimization operation at a fixed frequency diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h new file mode 100644 index 000000000..6fddc82f4 --- /dev/null +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h @@ -0,0 +1,116 @@ +/*************************************************************************** + * Copyright (C) 2019 Locus Robotics. All rights reserved. + * Unauthorized copying of this file, via any medium, is strictly prohibited + * Proprietary and confidential + ***************************************************************************/ +#ifndef FUSE_OPTIMIZERS_FIXED_LAG_SMOOTHER_PARAMS_H +#define FUSE_OPTIMIZERS_FIXED_LAG_SMOOTHER_PARAMS_H + +#include +#include +#include + +#include +#include +#include + + +namespace fuse_optimizers +{ + +/** + * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class + */ +struct FixedLagSmootherParams +{ +public: + /** + * @brief The set of sensors whose transactions will trigger the optimizer thread to start running + * + * This is designed to keep the system idle until the origin constraint has been received. + */ + std::vector ignition_sensors; + + /** + * @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 }; + + /** + * @brief Helper function that loads strictly positive floating point values from the parameter server + * + * @param[in] node_handle - The node handle used to load the parameter + * @param[in] parameter_name - The parameter name to load + * @param[in] default_value - A default value to use if the provided parameter name does not exist + * @return The loaded (or default) value + */ + double getPositiveParam(const ros::NodeHandle& node_handle, const std::string& parameter_name, double default_value) + { + double value; + node_handle.param(parameter_name, value, default_value); + if (value <= 0) + { + ROS_WARN_STREAM("The requested " << parameter_name << " is <= 0. Using the default value (" << + default_value << ") instead."); + value = default_value; + } + return value; + } + + /** + * @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 + nh.getParam("ignition_sensors", ignition_sensors); + std::sort(ignition_sensors.begin(), ignition_sensors.end()); + + auto lag_duration_sec = getPositiveParam(nh, "lag_duration", lag_duration.toSec()); + lag_duration.fromSec(lag_duration_sec); + + if (nh.hasParam("optimization_frequency")) + { + auto optimization_frequency = getPositiveParam(nh, "optimization_frequency", 1.0 / optimization_period.toSec()); + optimization_period.fromSec(1.0 / optimization_frequency); + } + else + { + auto optimization_period_sec = getPositiveParam(nh, "optimization_period", optimization_period.toSec()); + optimization_period.fromSec(optimization_period_sec); + } + + nh.getParam("reset_service", reset_service); + + auto transaction_timeout_sec = getPositiveParam(nh, "transaction_timeout", transaction_timeout.toSec()); + transaction_timeout.fromSec(transaction_timeout_sec); + } +}; + +} // namespace fuse_optimizers + +#endif // FUSE_OPTIMIZERS_FIXED_LAG_SMOOTHER_PARAMS_H diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a7f21fdec..3cd42c239 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -50,24 +51,6 @@ namespace fuse_optimizers { -namespace -{ - -double getPositiveParam(const ros::NodeHandle& node_handle, const std::string& parameter_name, double default_value) -{ - double value; - node_handle.param(parameter_name, value, default_value); - if (value <= 0) - { - ROS_WARN_STREAM("The requested " << parameter_name << " is <= 0. Using the default value (" << - default_value << ") instead."); - value = default_value; - } - return value; -} - -} // namespace - FixedLagSmoother::FixedLagSmoother( fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle, @@ -78,20 +61,11 @@ FixedLagSmoother::FixedLagSmoother( start_time_(ros::TIME_MAX), started_(false) { - auto lag_duration = getPositiveParam(private_node_handle_, "lag_duration", 5.0); - lag_duration_.fromSec(lag_duration); - - auto optimization_frequency = getPositiveParam(private_node_handle_, "optimization_frequency", 10.0); - optimization_period_ = ros::Duration(1.0 / optimization_frequency); + params_.loadFromROS(private_node_handle); - auto transaction_timeout = getPositiveParam(private_node_handle_, "transaction_timeout", 0.1); - transaction_timeout_.fromSec(transaction_timeout); - - private_node_handle_.getParam("ignition_sensors", ignition_sensors_); - // Sort the sensors for efficient lookups - std::sort(ignition_sensors_.begin(), ignition_sensors_.end()); // Warn about possible configuration errors - for (const auto& sensor_model_name : ignition_sensors_) + // TODO(swilliams) Move this warning to the Parameter loadFromROS() method once all parameters are loaded there. + for (const auto& sensor_model_name : params_.ignition_sensors) { if (sensor_models_.find(sensor_model_name) == sensor_models_.end()) { @@ -99,6 +73,7 @@ FixedLagSmoother::FixedLagSmoother( "model with that name currently exists. This is likely a configuration error."); } } + // Test for auto-start autostart(); @@ -107,12 +82,15 @@ FixedLagSmoother::FixedLagSmoother( // Configure a timer to trigger optimizations optimize_timer_ = node_handle_.createTimer( - optimization_period_, + params_.optimization_period, &FixedLagSmoother::optimizerTimerCallback, this); // Advertise a service that resets the optimizer to its initial state - reset_service_server_ = private_node_handle_.advertiseService("reset", &FixedLagSmoother::resetServiceCallback, this); + reset_service_server_ = node_handle_.advertiseService( + ros::names::resolve(params_.reset_service), + &FixedLagSmoother::resetServiceCallback, + this); } FixedLagSmoother::~FixedLagSmoother() @@ -129,7 +107,7 @@ FixedLagSmoother::~FixedLagSmoother() void FixedLagSmoother::autostart() { - if (ignition_sensors_.empty()) + if (params_.ignition_sensors.empty()) { // No ignition sensors were provided. Auto-start. started_ = true; @@ -147,9 +125,9 @@ std::vector FixedLagSmoother::computeVariablesToMarginalize() { auto current_stamp = timestamp_tracking_.currentStamp(); auto lag_stamp = ros::Time(0, 0); - if (current_stamp > ros::Time(0, 0) + lag_duration_) + if (current_stamp > ros::Time(0, 0) + params_.lag_duration) { - lag_stamp = current_stamp - lag_duration_; + lag_stamp = current_stamp - params_.lag_duration; } auto old_variables = std::vector(); timestamp_tracking_.query(lag_stamp, std::back_inserter(old_variables)); @@ -234,7 +212,7 @@ void FixedLagSmoother::optimizerTimerCallback(const ros::TimerEvent& event) { { std::lock_guard lock(optimization_requested_mutex_); - optimization_deadline_ = event.current_expected + optimization_period_; + optimization_deadline_ = event.current_expected + params_.optimization_period; } optimization_requested_.notify_one(); } @@ -257,13 +235,13 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction) // Apply the motion models to the transaction if (!applyMotionModels(element.sensor_name, *element.transaction)) { - if (element.transaction->stamp() + transaction_timeout_ < current_time) + 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 " - << transaction_timeout_ << ". Ignoring this transaction."); + << params_.transaction_timeout << ". Ignoring this transaction."); pending_transactions_.pop_back(); continue; } @@ -324,7 +302,7 @@ void FixedLagSmoother::transactionCallback( if (!started_) { // ...check if we should - if (std::binary_search(ignition_sensors_.begin(), ignition_sensors_.end(), sensor_name)) + if (std::binary_search(params_.ignition_sensors.begin(), params_.ignition_sensors.end(), sensor_name)) { started_ = true; start_time_ = transaction_time; @@ -338,9 +316,9 @@ void FixedLagSmoother::transactionCallback( { purge_time = start_time_; } - else if (ros::Time(0, 0) + transaction_timeout_ < last_pending_time) // ros::Time doesn't allow negatives + else if (ros::Time(0, 0) + params_.transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives { - purge_time = last_pending_time - transaction_timeout_; + purge_time = last_pending_time - params_.transaction_timeout; } while (!pending_transactions_.empty() && pending_transactions_.back_key() < purge_time) {