psm_node.cpp
Go to the documentation of this file.
00001 /*
00002 *  Polar Scan Matcher
00003 *  Copyright (C) 2010, CCNY Robotics Lab
00004 *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
00005 *  William Morris <morris@ee.ccny.cuny.edu>
00006 *  http://robotics.ccny.cuny.edu
00007 *  Modified 2014, Daniel Axtens <daniel@axtens.net>
00008 *  whilst a student at the Australian National University
00009 *
00010 *  This program is free software: you can redistribute it and/or modify
00011 *  it under the terms of the GNU General Public License as published by
00012 *  the Free Software Foundation, either version 3 of the License, or
00013 *  (at your option) any later version.
00014 *
00015 *  This program is distributed in the hope that it will be useful,
00016 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 *  GNU General Public License for more details.
00019 *
00020 *  You should have received a copy of the GNU General Public License
00021 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00022 *
00023 *  This is a wrapper around Polar Scan Matcher [1], written by 
00024 *  Albert Diosi
00025 *
00026 *  [1] A. Diosi and L. Kleeman, "Laser Scan Matching in Polar Coordinates with 
00027 *  Application to SLAM " Proceedings of 2005 IEEE/RSJ International Conference 
00028 *  on Intelligent Robots and Systems, August, 2005, Edmonton, Canada
00029 */
00030 
00031 #include "polar_scan_matcher/psm_node.h"
00032 
00033 int main (int argc, char** argv)
00034 {
00035   ros::init(argc, argv, "PolarScanMatching Node");
00036   PSMNode psmNode;
00037   ros::spin();
00038 }
00039 
00040 PSMNode::PSMNode()
00041 {
00042   ROS_INFO("Creating PolarScanMatching node");
00043 
00044   ros::NodeHandle nh;
00045 
00046   initialized_   = false;
00047   totalDuration_ = 0.0;
00048   scansCount_    = 0;
00049 
00050   prevWorldToBase_.setIdentity();
00051 
00052   getParams();  
00053 
00054   scanSubscriber_ = nh.subscribe (scanTopic_, 10, &PSMNode::scanCallback, this);
00055   imuSubscriber_  = nh.subscribe (imuTopic_,  10, &PSMNode::imuCallback,  this);
00056   posePublisher_  = nh.advertise<geometry_msgs::Pose2D>(poseTopic_, 10);
00057 }
00058 
00059 PSMNode::~PSMNode()
00060 {
00061   ROS_INFO("Destroying PolarScanMatching node");
00062 }
00063 
00064 void PSMNode::getParams()
00065 {
00066   ros::NodeHandle nh_private("~");
00067 
00068   std::string odometryType;
00069 
00070   // **** wrapper parameters
00071   
00072   if (!nh_private.getParam ("world_frame", worldFrame_))
00073     worldFrame_ = "world";
00074   if (!nh_private.getParam ("base_frame", baseFrame_))
00075     baseFrame_ = "base_link";
00076   if (!nh_private.getParam ("publish_tf", publishTf_))
00077     publishTf_ = true;
00078   if (!nh_private.getParam ("publish_pose", publishPose_))
00079     publishPose_ = true;
00080   if (!nh_private.getParam ("odometry_type", odometryType))
00081     odometryType = "none";
00082 
00083   if (odometryType.compare("none") == 0)
00084   {
00085     useTfOdometry_  = false;
00086     useImuOdometry_ = false;
00087   }
00088   else if (odometryType.compare("tf") == 0)
00089   {
00090     useTfOdometry_  = true;
00091     useImuOdometry_ = false;
00092   }
00093   else if (odometryType.compare("imu") == 0)
00094   {
00095     useTfOdometry_  = false;
00096     useImuOdometry_ = true;
00097   }
00098   else
00099   {
00100     ROS_WARN("Unknown value of odometry_type parameter passed to psm_node. \
00101               Using default value (\"none\")");
00102     useTfOdometry_  = false;
00103     useImuOdometry_ = false;
00104   }
00105 
00106   // **** PSM parameters
00107 
00108   if (!nh_private.getParam ("min_valid_points", minValidPoints_))
00109     minValidPoints_ = 200;
00110   if (!nh_private.getParam ("search_window", searchWindow_))
00111     searchWindow_ = 40;
00112   if (!nh_private.getParam ("max_error", maxError_))
00113     maxError_ = 0.20;
00114   if (!nh_private.getParam ("max_iterations", maxIterations_))
00115     maxIterations_ = 20;
00116   if (!nh_private.getParam ("stop_condition", stopCondition_))
00117     stopCondition_ = 0.01;
00118 }
00119 
00120 bool PSMNode::initialize(const sensor_msgs::LaserScan& scan)
00121 {
00122   laserFrame_ = scan.header.frame_id;
00123 
00124   // **** get base to laser tf
00125 
00126   tf::StampedTransform baseToLaserTf;
00127   try
00128   {
00129    tfListener_.waitForTransform(baseFrame_, scan.header.frame_id, scan.header.stamp, ros::Duration(1.0));
00130    tfListener_.lookupTransform (baseFrame_, scan.header.frame_id, scan.header.stamp, baseToLaserTf);
00131   }
00132   catch (tf::TransformException ex)
00133   {
00134     ROS_WARN("ScanMatcherNode: Could get initial laser transform, skipping scan (%s)", ex.what());
00135     return false;
00136   }
00137   baseToLaser_ = baseToLaserTf;
00138   laserToBase_ = baseToLaser_.inverse();
00139 
00140   // **** pass parameters to matcher and initialise
00141 
00142   matcher_.PM_L_POINTS         = scan.ranges.size();
00143 
00144   matcher_.PM_FOV              = (scan.angle_max - scan.angle_min) * 180.0 / M_PI;
00145   matcher_.PM_MAX_RANGE        = scan.range_max * ROS_TO_PM;
00146 
00147   matcher_.PM_TIME_DELAY       = 0.00;
00148 
00149   matcher_.PM_MIN_VALID_POINTS = minValidPoints_;
00150   matcher_.PM_SEARCH_WINDOW    = searchWindow_;
00151   matcher_.PM_MAX_ERROR        = maxError_ * ROS_TO_PM;
00152 
00153   matcher_.PM_MAX_ITER         = maxIterations_;
00154   matcher_.PM_MAX_ITER_ICP     = maxIterations_;
00155   matcher_.PM_STOP_COND        = stopCondition_ * ROS_TO_PM;
00156   matcher_.PM_STOP_COND_ICP    = stopCondition_ * ROS_TO_PM;
00157 
00158   matcher_.pm_init();
00159 
00160   // **** get the initial worldToBase tf
00161 
00162   getCurrentEstimatedPose(prevWorldToBase_, scan);
00163 
00164   // **** create the first pm scan from the laser scan message
00165 
00166   tf::Transform t;
00167   t.setIdentity();
00168   prevPMScan_ = new PMScan(scan.ranges.size());
00169   rosToPMScan(scan, t, prevPMScan_);
00170 
00171   return true;
00172 }
00173 
00174 void PSMNode::imuCallback (const sensor_msgs::Imu& imuMsg)
00175 {
00176   imuMutex_.lock();
00177   tf::Quaternion q(imuMsg.orientation.x, imuMsg.orientation.y, imuMsg.orientation.z, imuMsg.orientation.w);
00178   tf::Matrix3x3 m(q);
00179   double temp;
00180   m.getRPY(temp, temp, currImuAngle_);
00181   imuMutex_.unlock();
00182 }
00183 
00184 void PSMNode::scanCallback(const sensor_msgs::LaserScan& scan)
00185 {
00186   ROS_DEBUG("Received scan");
00187   scansCount_++;
00188 
00189   struct timeval start, end;
00190   gettimeofday(&start, NULL);
00191 
00192   // **** if this is the first scan, initialize and leave the function here
00193 
00194   if (!initialized_)
00195   {   
00196     initialized_ = initialize(scan);
00197     if (initialized_) ROS_INFO("Matcher initialized");
00198     return;
00199   }
00200   
00201   // **** attmempt to match the two scans
00202 
00203   // PM scan matcher is used in the following way:
00204   // The reference scan (prevPMScan_) always has a pose of 0
00205   // The new scan (currPMScan) has a pose equal to the movement
00206   // of the laser in the world frame since the last scan (tf::Transform change)
00207   // The computed correction is then propagated using the tf machinery
00208 
00209   prevPMScan_->rx = 0;
00210   prevPMScan_->ry = 0;
00211   prevPMScan_->th = 0; 
00212 
00213   tf::Transform currWorldToBase;
00214   tf::Transform change;
00215   change.setIdentity();
00216 
00217   // what odometry model to use
00218   if (useTfOdometry_) 
00219   {
00220     // get the current position of the base in the world frame
00221     // if no transofrm is available, we'll use the last known transform
00222 
00223     getCurrentEstimatedPose(currWorldToBase, scan);
00224     change = laserToBase_ * prevWorldToBase_.inverse() * currWorldToBase * baseToLaser_;
00225   }
00226   else if (useImuOdometry_)
00227   {
00228     imuMutex_.lock();
00229     double dTheta = currImuAngle_ - prevImuAngle_;
00230     prevImuAngle_ = currImuAngle_;
00231     change.getRotation().setRPY(0.0, 0.0, dTheta);
00232     imuMutex_.unlock();
00233   }
00234 
00235   PMScan * currPMScan = new PMScan(scan.ranges.size());
00236   rosToPMScan(scan, change, currPMScan);
00237   
00238   try
00239   {         
00240     matcher_.pm_psm(prevPMScan_, currPMScan);                         
00241   }
00242   catch(int err)
00243   {
00244     ROS_WARN("Error in scan matching");
00245     delete prevPMScan_;
00246     prevPMScan_ = currPMScan;
00247     return;
00248   };    
00249 
00250   // **** calculate change in position
00251 
00252   // rotate by -90 degrees, since polar scan matcher assumes different laser frame
00253   // and scale down by 100
00254   double dx =  currPMScan->ry / ROS_TO_PM;
00255   double dy = -currPMScan->rx / ROS_TO_PM;
00256   double da =  currPMScan->th; 
00257 
00258   // change = scan match result for how much laser moved between scans, 
00259   // in the world frame
00260   change.setOrigin(tf::Vector3(dx, dy, 0.0));
00261   tf::Quaternion q;
00262   q.setRPY(0, 0, da);
00263   change.setRotation(q);
00264   
00265   // **** publish the new estimated pose as a tf
00266    
00267   currWorldToBase = prevWorldToBase_ * baseToLaser_ * change * laserToBase_;
00268 
00269   if (publishTf_  ) publishTf  (currWorldToBase, scan.header.stamp);
00270   if (publishPose_) publishPose(currWorldToBase);
00271 
00272   // **** swap old and new
00273 
00274   delete prevPMScan_;
00275   prevPMScan_      = currPMScan;
00276   prevWorldToBase_ = currWorldToBase;
00277 
00278   // **** timing information - needed for profiling only
00279 
00280   gettimeofday(&end, NULL);
00281   double dur = ((end.tv_sec   * 1000000 + end.tv_usec  ) - 
00282                 (start.tv_sec * 1000000 + start.tv_usec)) / 1000.0;
00283   totalDuration_ += dur;
00284   double ave = totalDuration_/scansCount_;
00285 
00286   ROS_INFO("dur:\t %.3f ms \t ave:\t %.3f ms", dur, ave);
00287 }
00288 
00289 void PSMNode::publishTf(const tf::Transform& transform, 
00290                                   const ros::Time& time)
00291 {
00292   tf::StampedTransform transformMsg (transform, time, worldFrame_, baseFrame_);
00293   tfBroadcaster_.sendTransform (transformMsg);
00294 }
00295 
00296 void PSMNode::publishPose(const tf::Transform& transform)
00297 {
00298   geometry_msgs::Pose2D pose;
00299   tfToPose2D(transform, pose);
00300 
00301   posePublisher_.publish(pose);
00302 }
00303 
00304 void PSMNode::rosToPMScan(const sensor_msgs::LaserScan& scan, 
00305                           const tf::Transform& change,
00306                                 PMScan* pmScan)
00307 {
00308   geometry_msgs::Pose2D pose;
00309   tfToPose2D(change, pose);
00310 
00311   // FIXME: rotate x & y by 90 degree?
00312 
00313   pmScan->rx = pose.x * ROS_TO_PM;
00314   pmScan->ry = pose.y * ROS_TO_PM;
00315   pmScan->th = pose.theta;
00316 
00317   for (int i = 0; i < scan.ranges.size(); ++i)
00318   {
00319     if (scan.ranges[i] == 0) 
00320     {
00321       pmScan->r[i] = 99999;  // hokuyo uses 0 for out of range reading
00322     }
00323     else
00324     {
00325       pmScan->r[i] = scan.ranges[i] * ROS_TO_PM;
00326       pmScan->x[i] = (pmScan->r[i]) * matcher_.pm_co[i];
00327       pmScan->y[i] = (pmScan->r[i]) * matcher_.pm_si[i];
00328       pmScan->bad[i] = 0;
00329     }
00330 
00331     pmScan->bad[i] = 0;
00332   }
00333 
00334   matcher_.pm_median_filter  (pmScan);
00335   matcher_.pm_find_far_points(pmScan);
00336   matcher_.pm_segment_scan   (pmScan);  
00337 }
00338 
00339 void PSMNode::getCurrentEstimatedPose(tf::Transform& worldToBase, 
00340                                       const sensor_msgs::LaserScan& scanMsg)
00341 {
00342   tf::StampedTransform worldToBaseTf;
00343   try
00344   {
00345      tfListener_.lookupTransform (worldFrame_, baseFrame_, scanMsg.header.stamp, worldToBaseTf);
00346   }
00347   catch (tf::TransformException ex)
00348   {
00349     // transform unavailable - use the pose from our last estimation
00350     ROS_WARN("Transform unavailable, using last estimated pose (%s)", ex.what());
00351     worldToBase = prevWorldToBase_;
00352     return;
00353   }
00354   worldToBase = worldToBaseTf;
00355 }
00356 
00357 void PSMNode::pose2DToTf(const geometry_msgs::Pose2D& pose, tf::Transform& t)
00358 {
00359   t.setOrigin(tf::Vector3(pose.x, pose.y, 0.0));
00360   tf::Quaternion q;
00361   q.setRPY(0, 0, pose.theta);
00362   t.setRotation(q);
00363 }
00364 
00365 void PSMNode::tfToPose2D(const tf::Transform& t, geometry_msgs::Pose2D& pose)
00366 {
00367   tf::Matrix3x3 m(t.getRotation());
00368   double roll, pitch, yaw;
00369   m.getRPY(roll, pitch, yaw);
00370 
00371   pose.x = t.getOrigin().getX();
00372   pose.y = t.getOrigin().getY();
00373   pose.theta = yaw;
00374 }


polar_scan_matcher
Author(s): Ivan Dryanovski
autogenerated on Thu Mar 23 2017 03:34:38