Commit 6d9d38de authored by Tim Übelhör's avatar Tim Übelhör
Browse files

using time constant that is indepenent of the timer period

parent 24bb18b8
......@@ -4,8 +4,8 @@ from dynamic_reconfigure.parameter_generator_catkin import double_t
PACKAGE = "tf_low_pass"
gen = ParameterGenerator()
gen.add("smoothing_factor", double_t, 0,
"low factor leads to smoother signal", 0.1, 0, 1)
gen.add("time_constant", double_t, 0,
"high time constant leads to smoother signals", 0.2, 0)
gen.add("timer_period", double_t, 0,
"time between two filter steps", 0.01, 0)
"time between two filter steps", 0.02, 0)
exit(gen.generate(PACKAGE, "tf_low_pass", "tf_low_pass"))
#include <dynamic_reconfigure/server.h>
#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf_low_pass/tf_low_passConfig.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf_low_pass/tf_low_passConfig.h>
// declaration
class TfLowPass
{
public:
class TfLowPass {
public:
TfLowPass();
private:
private:
ros::NodeHandle nh;
ros::Timer timer;
ros::Duration timer_period;
dynamic_reconfigure::Server<tf_low_pass::tf_low_passConfig> server;
tf2_ros::Buffer tf_buffer;
tf2_ros::TransformListener tf_listener;
......@@ -23,7 +21,7 @@ private:
tf2::Transform last_transform;
ros::Time last_update;
double smoothing_factor;
double time_constant;
std::string frame_id, child_frame_id, filtered_frame_id;
void dynamic_reconfigure_callback(tf_low_pass::tf_low_passConfig &config,
......@@ -33,70 +31,56 @@ private:
// implementation
TfLowPass::TfLowPass() : tf_listener(tf_buffer),
last_update(0)
{
TfLowPass::TfLowPass() : tf_listener(tf_buffer) {
ros::NodeHandle pnh("~");
pnh.param<std::string>("frame_id", frame_id, "world");
pnh.param<std::string>("child_frame_id", child_frame_id, "object");
pnh.param<std::string>("child_frame_id", child_frame_id, "tracked_object");
pnh.param<std::string>("filtered_frame_id", filtered_frame_id,
"object_filtered");
server.setCallback(std::bind(&TfLowPass::dynamic_reconfigure_callback,
this,
std::placeholders::_1,
std::placeholders::_2));
"filtered_object");
server.setCallback(std::bind(&TfLowPass::dynamic_reconfigure_callback, this,
std::placeholders::_1, std::placeholders::_2));
ROS_INFO("dynamic reconfigure initialized");
timer = nh.createTimer(ros::Duration(0.1),
&TfLowPass::timer_callback,
this);
timer = nh.createTimer(ros::Duration(0.1), &TfLowPass::timer_callback, this);
ROS_INFO("timer initialized");
}
void TfLowPass::dynamic_reconfigure_callback(
tf_low_pass::tf_low_passConfig &config, uint32_t level)
{
smoothing_factor = config.smoothing_factor;
tf_low_pass::tf_low_passConfig &config, uint32_t level) {
time_constant = config.time_constant;
timer.setPeriod(ros::Duration(config.timer_period));
}
void TfLowPass::timer_callback(const ros::TimerEvent &timerEvent)
{
try
{
void TfLowPass::timer_callback(const ros::TimerEvent &timer_event) {
try {
// find transform for the expected timer time (steady intervals)
geometry_msgs::TransformStamped current_msg, last_msg;
current_msg = tf_buffer.lookupTransform(frame_id, child_frame_id,
ros::Time(0));
current_msg =
tf_buffer.lookupTransform(frame_id, child_frame_id, ros::Time(0));
tf2::Transform current_transform;
tf2::fromMsg(current_msg.transform, current_transform);
if (!last_update.isZero())
{
// apply exponential smoothing
// 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));
// publish
current_msg.transform = tf2::toMsg(current_transform);
current_msg.child_frame_id = filtered_frame_id;
tf_broadcaster.sendTransform(current_msg);
}
// publish the transform
current_msg.transform = tf2::toMsg(current_transform);
current_msg.child_frame_id = filtered_frame_id;
tf_broadcaster.sendTransform(current_msg);
// update last to current
last_transform = current_transform;
last_update = current_msg.header.stamp;
}
catch (tf2::TransformException &ex)
{
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
ros::Duration(1.0).sleep();
}
}
// init ros node in main
main(int argc, char *argv[])
{
main(int argc, char *argv[]) {
ros::init(argc, argv, "tf_low_pass");
TfLowPass tf_low_pass;
ros::spin();
......
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