[RST-2128] fixed lag smoother reset #61
Conversation
| @@ -268,6 +279,28 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction) | |||
| } | |||
| } | |||
|
|
|||
| bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) | |||
There was a problem hiding this comment.
Neither req nor res are used, so it might be good to make that clear here and in the header:
bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
| { | ||
| ROS_WARN_STREAM("Optimization exceeded the configured duration by " << | ||
| (optimization_complete - optimization_deadline) << "s"); | ||
| std::lock_guard<std::mutex> lock(optimization_mutex_); |
There was a problem hiding this comment.
I assume this is necessary because the reset request will come from another thread.
There was a problem hiding this comment.
Correct. Previously the graph_ was only modified by the optimizer thread. Now I am modifying the graph_ from the ROS callback thread as well.
| std::lock_guard<std::mutex> lock(pending_transactions_mutex_); | ||
| pending_transactions_.clear(); | ||
| } | ||
| // Clear the graph and marginal tracking states |
There was a problem hiding this comment.
And there's no weird state that we can get into from a transaction getting somehow inserted in between these two blocks? Just checking.
There was a problem hiding this comment.
I don't think so.
The ROS spinner is configured for a single thread. Since this is called from the ROS callback thread, no other ROS callbacks can fire while this is running. And thus new pending transactions cannot be added between blocks.
Added a "reset" service to the fixed-lag smoother. This reverts the smoother back to its original state, waiting for a transaction from the configured ignition sensors.