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

several fixes and parameter adjustments.

runs stable
parent 9b4d80e2
......@@ -5,5 +5,7 @@ PACKAGE = "tf_low_pass"
gen = ParameterGenerator()
gen.add("smoothing_factor", double_t, 0,
"low factor leads to smoother signal", 0.5, 0, 1)
"low factor leads to smoother signal", 0.1, 0, 1)
gen.add("timer_period", double_t, 0,
"time between two filter steps", 0.01, 0)
exit(gen.generate(PACKAGE, "tf_low_pass", "tf_low_pass"))
<launch>
<param name="use_sim_time" value="true"/>
<node name="tf_low_pass" pkg="tf_low_pass" type="tf_low_pass" output="screen">
<param name="frame_id" value="world"/>
<param name="child_frame_id" value="tracked_object"/>
<param name="filtered_frame_id" value="filterd_object"/>
</node>
</launch>
\ No newline at end of file
......@@ -14,12 +14,18 @@ public:
private:
ros::NodeHandle nh;
ros::Timer timer;
double smoothing_factor;
std::string frame_id, child_frame_id, filtered_frame_id;
ros::Duration timer_period;
dynamic_reconfigure::Server<tf_low_pass::tf_low_passConfig> server;
tf2_ros::Buffer tf_buffer;
tf2_ros::TransformListener tf_listener;
tf2_ros::TransformBroadcaster tf_broadcaster;
tf2::Transform last_transform;
ros::Time last_update;
double smoothing_factor;
std::string frame_id, child_frame_id, filtered_frame_id;
void dynamic_reconfigure_callback(tf_low_pass::tf_low_passConfig &config,
uint32_t level);
void timer_callback(const ros::TimerEvent &timerEvent);
......@@ -27,29 +33,30 @@ private:
// implementation
TfLowPass::TfLowPass() : tf_listener(tf_buffer)
TfLowPass::TfLowPass() : tf_listener(tf_buffer),
last_update(0)
{
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>("filtered_frame_id", filtered_frame_id,
"object_filtered");
double timer_duration = pnh.param("timer_duration", 0.01);
dynamic_reconfigure::Server<tf_low_pass::tf_low_passConfig> server;
server.setCallback(std::bind(&TfLowPass::dynamic_reconfigure_callback,
this,
std::placeholders::_1,
std::placeholders::_2));
timer = nh.createTimer(ros::Duration(timer_duration),
ROS_INFO("dynamic reconfigure initialized");
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;
timer.setPeriod(ros::Duration(config.timer_period));
}
void TfLowPass::timer_callback(const ros::TimerEvent &timerEvent)
......@@ -59,22 +66,25 @@ void TfLowPass::timer_callback(const ros::TimerEvent &timerEvent)
// 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,
timerEvent.current_expected);
last_msg = tf_buffer.lookupTransform(frame_id, child_frame_id,
timerEvent.last_expected);
tf2::Transform current, last;
tf2::fromMsg(current_msg.transform, current);
tf2::fromMsg(last_msg.transform, last);
// apply exponential smoothing
current.setRotation(last.getRotation().slerp(
current.getRotation(), smoothing_factor));
current.setOrigin(last.getOrigin().lerp(
current.getOrigin(), smoothing_factor));
ros::Time(0));
tf2::Transform current_transform;
tf2::fromMsg(current_msg.transform, current_transform);
if (!last_update.isZero())
{
// apply exponential smoothing
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);
tf_broadcaster.sendTransform(current_msg);
// publish
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)
{
......
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