test_scan_matcher.cpp
Go to the documentation of this file.
00001 /*********************************************************/
00009 #include <karto_scan_matcher/karto_scan_matcher.h>
00010 #include <geometry_msgs/Pose2D.h>
00011 #include <tf/transform_listener.h>
00012 #include <boost/circular_buffer.hpp>
00013 #include <string>
00014 #include <ros/time.h>
00015 #include <boost/thread.hpp>
00016 #include <boost/shared_ptr.hpp>
00017 
00018 namespace karto_scan_matcher
00019 {
00020 
00021 namespace sm=sensor_msgs;
00022 namespace gm=geometry_msgs;
00023 using std::string;
00024 using std::vector;
00025 
00026 using gm::Pose2D;
00027 
00028 typedef boost::circular_buffer<ScanWithPose> ScanBuffer;
00029 typedef boost::mutex::scoped_lock Lock;
00030 
00031 /************************************************************
00032  * Node class
00033  ************************************************************/
00034 
00035 class ScanMatcherTest
00036 {
00037 
00038 public:
00039 
00040   ScanMatcherTest ();
00041 
00042 private:
00043 
00044   /****************************************
00045    * Ops
00046    ****************************************/
00047   
00048   void scanCallback (const sm::LaserScan& scan);
00049   void attemptScanMatch (const ros::WallTimerEvent& e);
00050   Pose2D perturbPose (const Pose2D& pose);
00051   
00052   /****************************************
00053    * Params
00054    ****************************************/
00055 
00056   const unsigned scan_history_length_;
00057   const double noise_level_;
00058 
00059 
00060   /****************************************
00061    * State
00062    ****************************************/
00063 
00064   ScanBuffer scans_;
00065 
00066   /****************************************
00067    * Associated objects
00068    ****************************************/
00069  
00070   ros::NodeHandle nh_;
00071   tf::TransformListener tf_;
00072   ros::Subscriber scan_sub_;
00073   ros::WallTimer scan_match_timer_;
00074   boost::mutex mutex_;
00075   boost::shared_ptr<KartoScanMatcher> matcher_;
00076 
00077 };
00078 
00079 /************************************************************
00080  * Init
00081  ************************************************************/
00082 
00083 template <class T>
00084 T getPrivateParam(const string& name, const T& default_value)
00085 {
00086   ros::NodeHandle nh("~");
00087   T value;
00088   nh.param(name, value, default_value);
00089   return value;
00090 }
00091 
00092 
00093 ScanMatcherTest::ScanMatcherTest () :
00094   scan_history_length_(getPrivateParam<int>("scan_history_length", 10)), 
00095   noise_level_(getPrivateParam<double>("noise_level", 0.1)),
00096   scans_(scan_history_length_+1),
00097   scan_sub_(nh_.subscribe("base_scan", 10, &ScanMatcherTest::scanCallback, this)),
00098   scan_match_timer_(nh_.createWallTimer(ros::WallDuration(5.0), &ScanMatcherTest::attemptScanMatch, this))
00099 {
00100   ROS_ASSERT (scan_history_length_ > 1);
00101 }
00102 
00103 /************************************************************
00104  * Callbacks
00105  ************************************************************/
00106 
00107 void ScanMatcherTest::scanCallback (const sm::LaserScan& scan)
00108 {
00109   // If this is the first scan, initialize the matcher
00110   if (!matcher_.get()) {
00111     matcher_.reset(new KartoScanMatcher(scan, tf_, 0.3, 0.01));
00112     ROS_INFO ("Initialized matcher");
00113   }
00114 
00115 
00116   try {
00117 
00118     // Figure out current base position
00119     gm::PoseStamped identity;
00120     identity.header.frame_id = "base_link";
00121     identity.header.stamp = ros::Time();
00122     identity.pose.orientation.w = 1.0;
00123     identity.pose.orientation.x = identity.pose.orientation.y = identity.pose.orientation.z = 0.0;
00124     gm::PoseStamped map_pose;
00125     tf_.transformPose("/map", identity, map_pose);
00126   
00127     // Save it and the scan
00128     Lock lock(mutex_);
00129     ScanWithPose s;
00130     s.scan = scan;
00131     s.pose.x = map_pose.pose.position.x;
00132     s.pose.y = map_pose.pose.position.y;
00133     s.pose.theta = tf::getYaw(map_pose.pose.orientation);
00134     scans_.push_back(s);
00135   }
00136   catch (tf::LookupException& e) {
00137     ROS_INFO ("Not saving scan due to a tf lookup exception");
00138   }
00139 }
00140 
00141 
00142 
00143 /************************************************************
00144  * Scan matching
00145  ************************************************************/
00146 
00147 
00148 
00149 Pose2D ScanMatcherTest::perturbPose (const Pose2D& pose)
00150 {
00151   Pose2D perturbed;
00152   perturbed.x = pose.x + noise_level_;
00153   perturbed.y = pose.y - noise_level_;
00154   perturbed.theta = pose.theta + noise_level_;
00155   return perturbed;
00156 }
00157 
00158 void ScanMatcherTest::attemptScanMatch (const ros::WallTimerEvent& e)
00159 {
00160   ROS_INFO ("In attemptScanMatch.  Scan buffer has %zu elements", scans_.size());
00161   if (scans_.size() > scan_history_length_) {
00162 
00163     Lock lock(mutex_);
00164     Pose2D true_pose = scans_.back().pose;
00165     Pose2D perturbed_pose = perturbPose(true_pose);
00166 
00167     vector<ScanWithPose> reference_scans(scans_.begin(), scans_.end()-1);
00168 
00169     ROS_INFO_STREAM ("True pose is " << true_pose << " and perturbed pose is " << perturbed_pose);
00170     ROS_INFO_STREAM ("First reference scan is at " << reference_scans.begin()->pose << 
00171                      " and last one is at " << (reference_scans.end()-1)->pose);
00172     Pose2D estimated_pose = matcher_->scanMatch(scans_.back().scan, perturbed_pose, reference_scans).pose;
00173     ROS_INFO_STREAM ("Estimated pose is " << estimated_pose);
00174   }
00175 }
00176 
00177 
00178 } // namespace karto_scan_matcher
00179 
00180 
00181 
00182 int main (int argc, char** argv)
00183 {
00184   ros::init(argc, argv, "test_scan_matcher");
00185   karto_scan_matcher::ScanMatcherTest n;
00186   ros::spin();
00187   
00188 }
00189 
00190 


karto_scan_matcher
Author(s): Bhaskara Marthi
autogenerated on Thu Jan 2 2014 12:02:42