Skip to content

Commit

Permalink
formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@jakemclaughlin6
jakemclaughlin6 committed Jan 18, 2023
1 parent 157cd2d commit 70c03e5
Showing 1 changed file with 45 additions and 70 deletions.
115 changes: 45 additions & 70 deletions fuse_optimizers/src/fixed_lag_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,31 +50,19 @@

namespace fuse_optimizers
{
<<<<<<< HEAD

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 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<ParameterType>(private_node_handle)),
node_handle, private_node_handle)
{
}
>>>>>>> Address PR comments
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<ParameterType>(private_node_handle)),
node_handle, private_node_handle)
{
}

FixedLagSmoother::FixedLagSmoother(
fuse_core::Graph::UniquePtr graph,
Expand All @@ -92,66 +80,53 @@ FixedLagSmoother::FixedLagSmoother(fuse_core::Graph::UniquePtr graph, const ros:
timestamp_tracking_.addNewTransaction(new_transaction);
}

<<<<<<< HEAD
std::vector<fuse_core::UUID> FixedLagSmoother::computeVariablesToMarginalize()
{
// Find the most recent variable timestamp, then carefully subtract the lag duration.
// ROS Time objects do not handle negative values.
auto start_time = getStartTime();
=======
std::lock_guard<std::mutex> lock(mutex_);
auto now = timestamp_tracking_[timestamp_tracking_.numStates() - 1];
lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time;
auto marginalize_variable_uuids = std::vector<fuse_core::UUID>();
timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids));
return marginalize_variable_uuids;
}
>>>>>>> Address PR comments

std::lock_guard<std::mutex> lock(mutex_);
auto now = timestamp_tracking_.currentStamp();
lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time;
auto marginalize_variable_uuids = std::vector<fuse_core::UUID>();
timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids));
return marginalize_variable_uuids;
}
std::lock_guard<std::mutex> lock(mutex_);
auto now = timestamp_tracking_.currentStamp();
lag_expiration_ = (start_time + params_->lag_duration < now) ? now - params_->lag_duration : start_time;
auto marginalize_variable_uuids = std::vector<fuse_core::UUID>();
timestamp_tracking_.query(lag_expiration_, std::back_inserter(marginalize_variable_uuids));
return marginalize_variable_uuids;
}

void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_transaction)
{
<<<<<<< HEAD
std::lock_guard<std::mutex> lock(mutex_);
timestamp_tracking_.addMarginalTransaction(marginal_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;
>>>>>>> Address PR comments
}
void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction &marginal_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;
}

bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction)
bool FixedLagSmoother::validateTransaction(const std::string &sensor_name, const fuse_core::Transaction &transaction)
{
auto min_stamp = transaction.minStamp();
std::lock_guard<std::mutex> lock(mutex_);
if (min_stamp < lag_expiration_)
{
auto min_stamp = transaction.minStamp();
std::lock_guard<std::mutex> 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.");
return false;
}
return true;
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;
}

void FixedLagSmoother::onReset()
{
std::lock_guard<std::mutex> lock(mutex_);
timestamp_tracking_.clear();
lag_expiration_ = ros::Time(0, 0);
}
void FixedLagSmoother::onReset()
{
std::lock_guard<std::mutex> lock(mutex_);
timestamp_tracking_.clear();
lag_expiration_ = ros::Time(0, 0);
}

} // namespace fuse_optimizers

0 comments on commit 70c03e5

Please sign in to comment.