tf_distort.h
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 
00034 #ifndef TFDISTORT_H_
00035 #define TFDISTORT_H_
00036 
00037 #include <ros/ros.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <tf/transform_listener.h>
00040 
00041 #include <dynamic_reconfigure/server.h>
00042 #include <vicon_bridge/tf_distortConfig.h>
00043 
00044 #include <queue>
00045 #include <boost/thread.hpp>
00046 #include <boost/shared_ptr.hpp>
00047 
00048 #include <vicon_bridge/TfDistortInfo.h>
00049 
00050 namespace tf_distort{
00051 
00052 typedef dynamic_reconfigure::Server<vicon_bridge::tf_distortConfig> ReconfServer;
00053 typedef vicon_bridge::tf_distortConfig Config;
00054 
00055 class DelayedTransform
00056 {
00057 public:
00058   DelayedTransform();
00059   DelayedTransform(const tf::StampedTransform & _transform, const ros::Time & _time) :
00060     transform(_transform), time_to_publish(_time)
00061   {
00062   }
00063   ;
00064   tf::StampedTransform transform;
00065   ros::Time time_to_publish;
00066 };
00067 
00068 typedef std::queue<DelayedTransform> DelayedTransformQueue;
00069 
00070 const double rand_max_ = RAND_MAX;
00071 void whiteGaussianNoise(double * n1, double * n2 = NULL);
00072 
00073 class RandomWalk
00074 {
00075 private:
00076   double x_;
00077   double sigma_gm_;
00078   double tau_;
00079 public:
00080   RandomWalk() :
00081     x_(0), sigma_gm_(0), tau_(0)
00082   {
00083     //srand(time(NULL));
00084   }
00085   void configure(const double & sigma_gm, const double & tau)
00086   {
00087     sigma_gm_ = sigma_gm;
00088     tau_ = tau;
00089   }
00090   double update(const double & dt)
00091   {
00092     double n;
00093     whiteGaussianNoise(&n);
00094     x_ = n * sqrt(1.0 - exp(-dt / tau_)) + x_ * exp(-dt / tau_);
00095     return x_;
00096   }
00097   double get()
00098   {
00099     return x_;
00100   }
00101 };
00102 
00103 class TfDistort
00104 {
00105 private:
00106 
00107   ReconfServer* reconf_srv_;
00108   ros::NodeHandle nh_, pnh_;
00109   Config config_;
00110 
00111   ros::Duration pub_period_;
00112   ros::Duration delay_;
00113   ros::Time last_pub_time_;
00114 
00115   DelayedTransformQueue tf_queue_;
00116   boost::mutex tf_queue_mutex_;
00117 
00118   boost::signals::connection tf_cb_;
00119   tf::TransformBroadcaster tf_broadcaster;
00120   tf::TransformListener tf_listener_;
00121 //  boost::mutex pose_pub_mutex_;
00122   boost::shared_ptr<ros::Publisher> pose_pub_; 
00123 
00124   boost::thread pub_thread_;
00125   bool pub_thread_runnning_;
00126 
00127   RandomWalk random_walk_x_;
00128   RandomWalk random_walk_y_;
00129   RandomWalk random_walk_z_;
00130 
00131   ros::Publisher info_pub_;
00132 
00133   void startPubThread();
00134   void stopPubThread();
00135   void pubThread();
00136   void tfCb();
00137   void reconfCb(Config & config, uint32_t level);
00138   void addNoise(tf::StampedTransform & transform);
00139   void publishInfo(const Config & config);
00140 
00141 public:
00142   TfDistort();
00143   ~TfDistort();
00144   void poll();
00145 };
00146 
00147 }// end namespace tf_distort
00148 
00149 #endif /* TFDISTORT_H_ */


vicon_bridge
Author(s): Markus Achtelik
autogenerated on Sun Oct 5 2014 23:47:38