$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 * 00029 */ 00030 00039 #include <flirtlib_ros/flirtlib.h> 00040 #include <flirtlib_ros/conversions.h> 00041 #include <mongo_ros/message_collection.h> 00042 #include <geometry_msgs/PoseArray.h> 00043 #include <geometry_msgs/PoseStamped.h> 00044 #include <ros/ros.h> 00045 #include <tf/transform_listener.h> 00046 #include <boost/thread.hpp> 00047 #include <boost/foreach.hpp> 00048 #include <boost/lexical_cast.hpp> 00049 #include <boost/optional.hpp> 00050 00051 namespace flirtlib_ros 00052 { 00053 00054 namespace sm=sensor_msgs; 00055 namespace vm=visualization_msgs; 00056 namespace gm=geometry_msgs; 00057 namespace mr=mongo_ros; 00058 00059 using std::string; 00060 using std::vector; 00061 00062 typedef boost::mutex::scoped_lock Lock; 00063 typedef vector<InterestPoint*> InterestPointVec; 00064 typedef std::pair<InterestPoint*, InterestPoint*> Correspondence; 00065 typedef vector<Correspondence> Correspondences; 00066 typedef vector<RefScan> RefScans; 00067 00068 00069 00070 /************************************************************ 00071 * Node class 00072 ***********************************************************/ 00073 00074 class Node 00075 { 00076 public: 00077 00078 Node (); 00079 void scanCB (sm::LaserScan::ConstPtr scan); 00080 00081 private: 00082 00083 void initializeRefScans(); 00084 gm::Pose getPose(); 00085 00086 // Needed during initialization 00087 boost::mutex mutex_; 00088 ros::NodeHandle nh_; 00089 00090 // Parameters 00091 const double min_num_matches_; 00092 const tf::Transform laser_offset_; 00093 00094 // State 00095 RefScans ref_scans_; 00096 00097 // Flirtlib objects 00098 boost::shared_ptr<SimpleMinMaxPeakFinder> peak_finder_; 00099 boost::shared_ptr<HistogramDistance<double> > histogram_dist_; 00100 boost::shared_ptr<Detector> detector_; 00101 boost::shared_ptr<DescriptorGenerator> descriptor_; 00102 boost::shared_ptr<RansacFeatureSetMatcher> ransac_; 00103 00104 // Ros objects 00105 tf::TransformListener tf_; 00106 ros::Subscriber scan_sub_; 00107 ros::Publisher marker_pub_; 00108 ros::Publisher ref_scan_pose_pub_; 00109 ros::Publisher match_pose_pub_; 00110 ros::Publisher adjusted_pose_pub_; 00111 ros::Publisher pose_est_pub_; 00112 mr::MessageCollection<RefScanRos> scans_; 00113 00114 }; 00115 00116 00117 /************************************************************ 00118 * Initialization 00119 ***********************************************************/ 00120 00121 template <class T> 00122 T getPrivateParam (const string& name) 00123 { 00124 ros::NodeHandle nh("~"); 00125 T val; 00126 const bool found = nh.getParam(name, val); 00127 ROS_ASSERT_MSG (found, "Could not find parameter %s", name.c_str()); 00128 ROS_DEBUG_STREAM_NAMED ("init", "Initialized " << name << " to " << val); 00129 return val; 00130 } 00131 00132 template <class T> 00133 T getPrivateParam (const string& name, const T& default_val) 00134 { 00135 ros::NodeHandle nh("~"); 00136 T val; 00137 nh.param(name, val, default_val); 00138 ROS_DEBUG_STREAM_NAMED ("init", "Initialized " << name << " to " << val << 00139 "(default was " << default_val << ")"); 00140 return val; 00141 } 00142 00143 SimpleMinMaxPeakFinder* createPeakFinder () 00144 { 00145 return new SimpleMinMaxPeakFinder(0.34, 0.001); 00146 } 00147 00148 Detector* createDetector (SimpleMinMaxPeakFinder* peak_finder) 00149 { 00150 const double scale = 7.0; 00151 const double dmst = 2.0; 00152 const double base_sigma = 0.1; 00153 const double sigma_step = 1.4; 00154 CurvatureDetector* det = new CurvatureDetector(peak_finder, scale, base_sigma, 00155 sigma_step, dmst); 00156 det->setUseMaxRange(false); 00157 return det; 00158 } 00159 00160 DescriptorGenerator* createDescriptor (HistogramDistance<double>* dist) 00161 { 00162 const double min_rho = 0.02; 00163 const double max_rho = 0.5; 00164 const double bin_rho = 4; 00165 const double bin_phi = 12; 00166 BetaGridGenerator* gen = new BetaGridGenerator(min_rho, max_rho, bin_rho, 00167 bin_phi); 00168 gen->setDistanceFunction(dist); 00169 return gen; 00170 } 00171 00172 tf::Transform loadLaserOffset () 00173 { 00174 const double yaw = getPrivateParam<double>("laser_offset_yaw", 0.0); 00175 const double x = getPrivateParam<double>("laser_offset_x", 0.0); 00176 const double y = getPrivateParam<double>("laser_offset_y", 0.0); 00177 return tf::Transform(tf::createQuaternionFromYaw(yaw), tf::Vector3(x, y, 0)); 00178 } 00179 00180 Node::Node () : 00181 00182 min_num_matches_(getPrivateParam<int>("min_num_matches", 10)), 00183 laser_offset_(loadLaserOffset()), 00184 00185 peak_finder_(createPeakFinder()), 00186 histogram_dist_(new SymmetricChi2Distance<double>()), 00187 detector_(createDetector(peak_finder_.get())), 00188 descriptor_(createDescriptor(histogram_dist_.get())), 00189 ransac_(new RansacFeatureSetMatcher(0.0599, 0.98, 0.4, 0.4, 00190 0.0384, false)), 00191 scan_sub_(nh_.subscribe("scan", 1, &Node::scanCB, this)), 00192 marker_pub_(nh_.advertise<vm::Marker>("visualization_marker", 10)), 00193 ref_scan_pose_pub_(nh_.advertise<gm::PoseArray>("ref_scan_poses", 10, true)), 00194 match_pose_pub_(nh_.advertise<gm::PoseArray>("match_poses", 1)), 00195 adjusted_pose_pub_(nh_.advertise<gm::PoseArray>("adjusted_poses", 1)), 00196 pose_est_pub_(nh_.advertise<gm::PoseStamped>("pose_estimate", 1)), 00197 scans_(getPrivateParam<string>("scan_db"), "scans") 00198 { 00199 ROS_DEBUG_NAMED ("init", "Db contains %u scans.", 00200 scans_.count()); 00201 initializeRefScans(); 00202 ROS_INFO ("Startup loc initialized"); 00203 } 00204 00205 gm::Pose makePose (const double x, const double y, const double theta) 00206 { 00207 gm::Pose p; 00208 p.position.x = x; 00209 p.position.y = y; 00210 p.orientation = tf::createQuaternionMsgFromYaw(theta); 00211 return p; 00212 } 00213 00214 void Node::initializeRefScans () 00215 { 00216 gm::PoseArray poses; 00217 poses.header.stamp = ros::Time::now(); 00218 poses.header.frame_id = "/map"; 00219 unsigned i=0; 00220 00221 BOOST_FOREACH (mr::MessageWithMetadata<RefScanRos>::ConstPtr m, 00222 scans_.queryResults(mr::Query(), false)) 00223 { 00224 ROS_DEBUG_NAMED("init", "Reading scan %u", i); 00225 ref_scans_.push_back(fromRos(*m)); 00226 poses.poses.push_back(makePose(m->lookupDouble("x"), m->lookupDouble("y"), 00227 m->lookupDouble("theta"))); 00228 } 00229 ref_scan_pose_pub_.publish(poses); 00230 } 00231 00232 00233 /************************************************************ 00234 * Main 00235 ***********************************************************/ 00236 00237 00238 gm::Pose Node::getPose () 00239 { 00240 tf::StampedTransform trans; 00241 tf_.lookupTransform("/map", "base_laser_link", ros::Time(), trans); 00242 gm::Pose pose; 00243 tf::poseTFToMsg(trans, pose); 00244 return pose; 00245 } 00246 00247 gm::Pose transformPose (const gm::Pose& p, const OrientedPoint2D& trans) 00248 { 00249 tf::Transform laser_pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.275, 0, 0)); 00250 tf::Transform tr; 00251 tf::poseMsgToTF(p, tr); 00252 tf::Transform rel(tf::createQuaternionFromYaw(trans.theta), 00253 tf::Vector3(trans.x, trans.y, 0.0)); 00254 gm::Pose ret; 00255 tf::poseTFToMsg(tr*rel*laser_pose, ret); 00256 return ret; 00257 } 00258 00259 gm::Pose transformPose (const tf::Transform& trans, const gm::Pose& pose) 00260 { 00261 tf::Transform p; 00262 tf::poseMsgToTF(pose, p); 00263 gm::Pose ret; 00264 tf::poseTFToMsg(trans*p, ret); 00265 return ret; 00266 } 00267 00268 00269 00270 void Node::scanCB (sm::LaserScan::ConstPtr scan) 00271 { 00272 try 00273 { 00274 // Getting pose is the part that can throw exceptions 00275 const gm::Pose current_pose = getPose(); 00276 const double theta = tf::getYaw(current_pose.orientation); 00277 const double x=current_pose.position.x; 00278 const double y=current_pose.position.y; 00279 00280 // Extract features for this scan 00281 InterestPointVec pts; 00282 boost::shared_ptr<LaserReading> reading = fromRos(*scan); 00283 detector_->detect(*reading, pts); 00284 BOOST_FOREACH (InterestPoint* p, pts) 00285 p->setDescriptor(descriptor_->describe(*p, *reading)); 00286 marker_pub_.publish(interestPointMarkers(pts, current_pose, 0)); 00287 00288 // Match 00289 gm::PoseArray match_poses; 00290 int best_num_matches = -1; 00291 gm::PoseArray adjusted_poses; 00292 gm::PoseStamped best_pose; 00293 best_pose.header.frame_id = adjusted_poses.header.frame_id = 00294 match_poses.header.frame_id = "/map"; 00295 best_pose.header.stamp = adjusted_poses.header.stamp = 00296 match_poses.header.stamp = ros::Time::now(); 00297 ROS_DEBUG_NAMED ("match", "Matching scan at %.2f, %.2f, %.2f", x, y, theta); 00298 BOOST_FOREACH (const RefScan& ref_scan, ref_scans_) 00299 { 00300 Correspondences matches; 00301 OrientedPoint2D trans; 00302 ransac_->matchSets(ref_scan.raw_pts, pts, trans, matches); 00303 const int num_matches = matches.size(); 00304 if (num_matches > min_num_matches_) 00305 { 00306 ROS_DEBUG_NAMED ("match", "Found %d matches with ref scan at " 00307 "%.2f, %.2f, %.2f", num_matches, 00308 ref_scan.pose.position.x, ref_scan.pose.position.y, 00309 tf::getYaw(ref_scan.pose.orientation)); 00310 match_poses.poses.push_back(ref_scan.pose); 00311 const gm::Pose laser_pose = transformPose(ref_scan.pose, trans); 00312 const gm::Pose adjusted_pose = transformPose(laser_offset_, laser_pose); 00313 if (num_matches > best_num_matches) 00314 { 00315 best_num_matches = num_matches; 00316 ROS_DEBUG_NAMED ("match", "Transform is %.2f, %.2f, %.2f." 00317 " Transformed pose is %.2f, %.2f, %.2f", 00318 trans.x, trans.y, trans.theta, 00319 adjusted_pose.position.x, adjusted_pose.position.y, 00320 tf::getYaw(adjusted_pose.orientation)); 00321 best_pose.pose = adjusted_pose; 00322 } 00323 } 00324 } 00325 if (best_num_matches >= min_num_matches_) 00326 { 00327 match_pose_pub_.publish(match_poses); 00328 adjusted_pose_pub_.publish(adjusted_poses); 00329 pose_est_pub_.publish(best_pose); 00330 } 00331 } 00332 00333 catch (tf::TransformException& e) 00334 { 00335 ROS_INFO ("Skipping because of tf exception"); 00336 } 00337 } 00338 00339 00340 } // namespace 00341 00342 int main (int argc, char** argv) 00343 { 00344 ros::init(argc, argv, "startup_loc"); 00345 flirtlib_ros::Node node; 00346 ros::spin(); 00347 return 0; 00348 }