tf_distort.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 
00033 #include "tf_distort.h"
00034 
00035 namespace tf_distort{
00036 
00037 inline void whiteGaussianNoise(double * n1, double * n2)
00038 {
00039   // Box Muller Method - from http://www.dspguru.com/dsp/howtos/how-to-generate-white-gaussian-noise
00040   double v1, v2, s;
00041 
00042   do
00043   {
00044     v1 = 2 * static_cast<double> (rand()) / rand_max_ - 1.0;
00045     v2 = 2 * static_cast<double> (rand()) / rand_max_ - 1.0;
00046     s = v1 * v1 + v2 * v2;
00047   } while (s >= 1);
00048 
00049   *n1 = sqrt(-2.0 * log(s) / s) * v1;
00050   if (n2)
00051     *n2 = sqrt(-2.0 * log(s) / s) * v2;
00052 }
00053 
00054 inline double uniformNoise(const double & mag = 1)
00055 {
00056   return (static_cast<double> (rand()) / rand_max_ * 2.0 - 1.0) * mag;
00057 }
00058 
00059 TfDistort::TfDistort() :
00060   nh_(""), pnh_("~"), pub_thread_runnning_(false)
00061 {
00062   srand(ros::Time::now().toNSec());
00063   info_pub_ = nh_.advertise<vicon_bridge::TfDistortInfo>("tf_distort/info", 1);
00064 
00065   // bring up dynamic reconfigure
00066   reconf_srv_ = new ReconfServer(pnh_);
00067   reconf_srv_->setCallback(boost::bind(&TfDistort::reconfCb, this, _1, _2));
00068 
00069   startPubThread();
00070 }
00071 
00072 TfDistort::~TfDistort()
00073 {
00074   stopPubThread();
00075   delete reconf_srv_;
00076 }
00077 
00078 void TfDistort::startPubThread()
00079 {
00080   if (!pub_thread_runnning_)
00081   {
00082     pub_thread_runnning_ = true;
00083     pub_thread_ = boost::thread(&TfDistort::pubThread, this);
00084   }
00085 }
00086 
00087 void TfDistort::stopPubThread()
00088 {
00089   pub_thread_runnning_ = false;
00090   pub_thread_.join();
00091 }
00092 
00093 void TfDistort::addNoise(tf::StampedTransform & tf)
00094 {
00095   const double deg2rad = M_PI / 180.0;
00096   static ros::Time last_time = ros::Time(0);
00097   static bool once = false;
00098 
00099   if (!once)
00100   {
00101     once = true;
00102     last_time = tf.stamp_;
00103     return;
00104   }
00105 
00106   double dt = (tf.stamp_-last_time).toSec();
00107   double noise_x = 0, noise_y = 0, noise_z = 0, noise_roll = 0, noise_pitch = 0, noise_yaw = 0;
00108 
00109   if (config_.noise_type == vicon_bridge::tf_distort_NORMAL)
00110   {
00111     whiteGaussianNoise(&noise_roll, &noise_pitch);
00112     whiteGaussianNoise(&noise_yaw, &noise_x);
00113     whiteGaussianNoise(&noise_y, &noise_z);
00114 
00115     noise_roll *= config_.sigma_roll_pitch * deg2rad;
00116     noise_pitch *= config_.sigma_roll_pitch * deg2rad;
00117     noise_yaw *= config_.sigma_yaw * deg2rad;
00118 
00119     noise_x *= config_.sigma_xy;
00120     noise_y *= config_.sigma_xy;
00121     noise_z *= config_.sigma_z;
00122   }
00123   else if (config_.noise_type == vicon_bridge::tf_distort_UNIFORM)
00124   {
00125     noise_roll = uniformNoise(config_.sigma_roll_pitch * deg2rad);
00126     noise_pitch = uniformNoise(config_.sigma_roll_pitch * deg2rad);
00127     noise_yaw = uniformNoise(config_.sigma_yaw * deg2rad);
00128     noise_x = uniformNoise(config_.sigma_xy);
00129     noise_y = uniformNoise(config_.sigma_xy);
00130     noise_z = uniformNoise(config_.sigma_z);
00131   }
00132 
00133   noise_x += random_walk_x_.update(dt) * config_.random_walk_k_xy;
00134   noise_y += random_walk_y_.update(dt) * config_.random_walk_k_xy;
00135   noise_z += random_walk_z_.update(dt) * config_.random_walk_k_z;
00136 
00137   tf::Vector3 & p = tf.getOrigin();
00138   p.setX(p.x() + noise_x);
00139   p.setY(p.y() + noise_y);
00140   p.setZ(p.z() + noise_z);
00141 
00142   p *= config_.position_scale;
00143 
00144   tf::Quaternion q;
00145   q.setRPY(noise_roll, noise_pitch, noise_yaw);
00146   tf.setRotation(tf.getRotation() * q);
00147 
00148   last_time = tf.stamp_;
00149 }
00150 
00151 
00152 void TfDistort::reconfCb(Config & config, uint32_t level)
00153 {
00154   if(tf_cb_.connected())
00155     tf_cb_.disconnect();
00156 
00157   pub_period_.fromSec(1.0/config.tf_pub_rate);
00158   delay_.fromNSec(config.delay*1e6);
00159 
00160   random_walk_x_.configure(config.random_walk_sigma_xy, config.random_walk_tau_xy);
00161   random_walk_y_.configure(config.random_walk_sigma_xy, config.random_walk_tau_xy);
00162   random_walk_z_.configure(config.random_walk_sigma_z, config.random_walk_tau_z);
00163 
00164   if(config.tf_publish && !tf_cb_.connected()){
00165     tf_cb_ = tf_listener_.addTransformsChangedListener(boost::bind(&TfDistort::tfCb, this));
00166   }
00167 
00168   if(config.tf_frame_out != config_.tf_frame_out){
00169     ros::NodeHandle nh;
00170     stopPubThread();
00171     pose_pub_.reset(new ros::Publisher);
00172     *pose_pub_= nh.advertise<geometry_msgs::TransformStamped>(config.tf_frame_out, 1);
00173     startPubThread();
00174   }
00175 
00176   config_ = config;
00177   publishInfo(config_);
00178 }
00179 
00180 
00181 void TfDistort::tfCb()
00182 {
00183   tf::StampedTransform pose;
00184   static tf::StampedTransform last_pose;
00185   ros::Time tf_time(0);
00186 
00187   if (tf_listener_.canTransform(config_.tf_ref_frame, config_.tf_frame_in, tf_time))
00188   {
00189     tf_listener_.lookupTransform(config_.tf_ref_frame, config_.tf_frame_in, tf_time, pose);
00190 
00191     // check for new pose
00192     if (pose.getOrigin() != last_pose.getOrigin())
00193     {
00194       last_pose = pose;
00195 //      ros::Time time_now = ros::Time::now();
00196       ros::Time time_now = pose.stamp_;
00197 
00198       if ((time_now - last_pub_time_) > pub_period_)
00199       {
00200         pose.child_frame_id_ = config_.tf_frame_out;
00201         boost::mutex::scoped_lock(tf_queue_mutex_);
00202         tf_queue_.push(DelayedTransform(pose, time_now + delay_));
00203         last_pub_time_ = time_now;
00204       }
00205 
00206     }
00207   }
00208 }
00209 
00210 void TfDistort::pubThread()
00211 {
00212 //  ros::Duration d(0.005);
00213   ros::Duration d(0.001);
00214   ros::Time time_now;
00215   uint32_t cnt = 0;
00216   uint32_t msg_cnt = 0;
00217   geometry_msgs::TransformStamped pose;
00218 
00219   while (nh_.ok() && pub_thread_runnning_)
00220   {
00221     cnt++;
00222     d.sleep();
00223     boost::mutex::scoped_lock lock(tf_queue_mutex_);
00224 
00225     if (tf_queue_.empty())
00226       continue;
00227 
00228     DelayedTransform & dt = tf_queue_.front();
00229     time_now = ros::Time::now();
00230 
00231     if (std::abs(time_now.toSec() - dt.time_to_publish.toSec()) < d.toSec() * 0.75
00232         || dt.time_to_publish.toSec() - time_now.toSec() < 0)
00233     {
00234       addNoise(dt.transform);
00235       tf_broadcaster.sendTransform(dt.transform);
00236       tf::transformStampedTFToMsg(dt.transform, pose);
00237 //      {
00238 //        boost::mutex::scoped_try_lock lock(pose_pub_mutex_);
00239 //        if(lock.owns_lock())
00240           pose_pub_->publish(pose);
00241 //      }
00242 
00243       tf_queue_.pop();
00244       msg_cnt++;
00245     }
00246     if (cnt > 1.0 / d.toSec())
00247     {
00248 //      ROS_INFO("queue size: %d publishing at %d Hz", tf_queue_.size(), msg_cnt);
00249       msg_cnt = 0;
00250       cnt = 0;
00251 
00252       publishInfo(config_);
00253     }
00254   }
00255 }
00256 
00257 void TfDistort::publishInfo(const Config & config){
00258   vicon_bridge::TfDistortInfoPtr info(new vicon_bridge::TfDistortInfo);
00259   info->delay = config.delay;
00260   info->noise_type = config.noise_type;
00261   info->position_scale = config.position_scale;
00262   info->random_walk_k_xy = config.random_walk_k_xy;
00263   info->random_walk_k_z = config.random_walk_k_z;
00264   info->random_walk_sigma_xy = config.random_walk_sigma_xy;
00265   info->random_walk_sigma_z = config.random_walk_sigma_z;
00266   info->random_walk_tau_xy = config.random_walk_tau_xy;
00267   info->random_walk_tau_z = config.random_walk_tau_z;
00268   info->sigma_roll_pitch = config.sigma_roll_pitch;
00269   info->sigma_xy = config.sigma_xy;
00270   info->sigma_yaw = config.sigma_yaw;
00271   info->sigma_z = config.sigma_z;
00272   info->tf_frame_in = config.tf_frame_in;
00273   info->tf_frame_out = config.tf_frame_out;
00274   info->tf_pub_rate = config.tf_pub_rate;
00275   info->tf_ref_frame = config.tf_ref_frame;
00276 
00277   info_pub_.publish(info);
00278 }
00279 
00280 }// end namespace tf_distort
00281 
00282 int main(int argc, char** argv)
00283 {
00284 
00285   ros::init(argc, argv, "tf_distort"/*, ros::init_options::AnonymousName*/);
00286 
00287   tf_distort::TfDistort tfd;
00288 
00289   ros::spin();
00290 
00291   return 0;
00292 }
00293 
00294 


vicon_bridge
Author(s):
autogenerated on Mon Sep 14 2015 03:57:50