Commit 5edd433d authored by Tim Übelhör's avatar Tim Übelhör
Browse files

Fixed NaNs

parent e80797c8
......@@ -47,7 +47,9 @@ TfLowPass::TfLowPass() : tf_listener(tf_buffer) {
void TfLowPass::dynamic_reconfigure_callback(
tf_low_pass::tf_low_passConfig &config, uint32_t level) {
time_constant = config.time_constant;
if (config.time_constant > 0) {
time_constant = config.time_constant;
}
timer.setPeriod(ros::Duration(config.timer_period));
}
......@@ -62,11 +64,13 @@ void TfLowPass::timer_callback(const ros::TimerEvent &timer_event) {
// apply exponential smoothing
if (last_update.toSec() > 0) {
double delta_t = (current_msg.header.stamp - last_update).toSec();
double smoothing_factor = 1 - std::exp(-delta_t / time_constant);
current_transform.setRotation(last_transform.getRotation().slerp(
current_transform.getRotation(), smoothing_factor));
current_transform.setOrigin(last_transform.getOrigin().lerp(
current_transform.getOrigin(), smoothing_factor));
if (delta_t > 0) {
double smoothing_factor = 1 - std::exp(-delta_t / time_constant);
current_transform.setRotation(last_transform.getRotation().slerp(
current_transform.getRotation(), smoothing_factor));
current_transform.setOrigin(last_transform.getOrigin().lerp(
current_transform.getOrigin(), smoothing_factor));
}
}
// publish the transform
current_msg.transform = tf2::toMsg(current_transform);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment