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


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