[RST-2186] Added a FixedLagSmootherParams struct #68
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <ros/console.h> | ||
| #include <ros/duration.h> | ||
| #include <ros/node_handle.h> | ||
|
|
||
| #include <algorithm> | ||
| #include <string> | ||
| #include <vector> | ||
|
|
||
|
|
||
| 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<std::string> 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 | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -41,6 +41,7 @@ | |
| #include <ros/ros.h> | ||
|
|
||
| #include <algorithm> | ||
| #include <iterator> | ||
| #include <mutex> | ||
| #include <string> | ||
| #include <thread> | ||
|
|
@@ -50,24 +51,6 @@ | |
| namespace fuse_optimizers | ||
| { | ||
|
|
||
| FixedLagSmoother::FixedLagSmoother( | ||
| fuse_core::Graph::UniquePtr graph, | ||
| const ros::NodeHandle& node_handle, | ||
|
|
@@ -78,27 +61,19 @@ FixedLagSmoother::FixedLagSmoother( | |
| start_time_(ros::TIME_MAX), | ||
| started_(false) | ||
| { | ||
| params_.loadFromROS(private_node_handle); | ||
|
|
||
| // Warn about possible configuration errors | ||
| // 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()) | ||
| { | ||
| ROS_WARN_STREAM("Sensor '" << sensor_model_name << "' is configured as an ignition sensor, but no sensor " | ||
| "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( | ||
| params_.optimization_period, | ||
| &FixedLagSmoother::optimizerTimerCallback, | ||
| this); | ||
|
|
||
| // Advertise a service that resets the optimizer to its initial state | ||
| reset_service_server_ = node_handle_.advertiseService( | ||
|
There was a problem hiding this comment. This switch from |
||
| ros::names::resolve(params_.reset_service), | ||
| &FixedLagSmoother::resetServiceCallback, | ||
| this); | ||
| } | ||
|
|
||
| FixedLagSmoother::~FixedLagSmoother() | ||
|
|
@@ -129,7 +107,7 @@ FixedLagSmoother::~FixedLagSmoother() | |
|
|
||
| void FixedLagSmoother::autostart() | ||
| { | ||
| if (params_.ignition_sensors.empty()) | ||
| { | ||
| // No ignition sensors were provided. Auto-start. | ||
| started_ = true; | ||
|
|
@@ -147,9 +125,9 @@ std::vector<fuse_core::UUID> FixedLagSmoother::computeVariablesToMarginalize() | |
| { | ||
| auto current_stamp = timestamp_tracking_.currentStamp(); | ||
| auto lag_stamp = ros::Time(0, 0); | ||
| if (current_stamp > ros::Time(0, 0) + params_.lag_duration) | ||
| { | ||
| lag_stamp = current_stamp - params_.lag_duration; | ||
| } | ||
| auto old_variables = std::vector<fuse_core::UUID>(); | ||
| timestamp_tracking_.query(lag_stamp, std::back_inserter(old_variables)); | ||
|
|
@@ -234,7 +212,7 @@ void FixedLagSmoother::optimizerTimerCallback(const ros::TimerEvent& event) | |
| { | ||
| { | ||
| std::lock_guard<std::mutex> lock(optimization_requested_mutex_); | ||
| 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() + 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."); | ||
| pending_transactions_.pop_back(); | ||
| continue; | ||
| } | ||
|
|
@@ -324,7 +302,7 @@ void FixedLagSmoother::transactionCallback( | |
| if (!started_) | ||
| { | ||
| // ...check if we should | ||
| 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) + params_.transaction_timeout < last_pending_time) // ros::Time doesn't allow negatives | ||
| { | ||
| purge_time = last_pending_time - params_.transaction_timeout; | ||
| } | ||
| while (!pending_transactions_.empty() && pending_transactions_.back_key() < purge_time) | ||
| { | ||
|
|
||
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Not for this PR, but if we can't release some of our internal utility methods like this, we should consider adding some of them to a utility header in this package.