psm_node.h
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 #ifndef POLAR_SCAN_MATCHER_PSM_NODE_H
00032 #define POLAR_SCAN_MATCHER_PSM_NODE_H
00033 
00034 #include <sys/time.h>
00035 
00036 #include <ros/ros.h>
00037 #include <tf/transform_broadcaster.h>
00038 #include <tf/transform_listener.h>
00039 #include <sensor_msgs/LaserScan.h>
00040 #include <sensor_msgs/Imu.h>
00041 #include <geometry_msgs/Pose2D.h>
00042 
00043 #include "polar_scan_matcher/polar_match.h"
00044 
00045 const std::string imuTopic_  = "imu";
00046 const std::string scanTopic_ = "scan";
00047 const std::string poseTopic_ = "pose2D";
00048 
00049 const double ROS_TO_PM = 100.0;   // convert from cm to m
00050 
00051 class PSMNode
00052 {
00053   private:
00054 
00055     ros::Subscriber scanSubscriber_;
00056     ros::Subscriber imuSubscriber_;
00057     ros::Publisher  posePublisher_;
00058 
00059     tf::TransformBroadcaster tfBroadcaster_;
00060     tf::TransformListener    tfListener_;
00061     tf::Transform prevWorldToBase_;
00062     tf::Transform baseToLaser_;
00063     tf::Transform laserToBase_;
00064 
00065     bool initialized_;
00066     double totalDuration_;
00067     int scansCount_;
00068 
00069     PolarMatcher matcher_;
00070     PMScan * prevPMScan_;
00071 
00072     boost::mutex imuMutex_;
00073     double prevImuAngle_;   // the yaw angle when we last perfomred a scan match
00074     double currImuAngle_;   // the most recent yaw angle we have received
00075 
00076     // **** parameters
00077 
00078     bool   publishTf_;
00079     bool   publishPose_;
00080     bool   useTfOdometry_;
00081     bool   useImuOdometry_;
00082 
00083     int    minValidPoints_;
00084     int    searchWindow_;
00085     double maxError_;
00086     int    maxIterations_;
00087     double stopCondition_;
00088 
00089     std::string worldFrame_;
00090     std::string baseFrame_;
00091     std::string laserFrame_;
00092 
00093     void getParams();
00094     bool initialize(const sensor_msgs::LaserScan& scan);
00095 
00096     void imuCallback (const sensor_msgs::Imu& imuMsg);
00097     void scanCallback (const sensor_msgs::LaserScan& scan);
00098 
00099     void publishTf(const tf::Transform& transform, 
00100                    const ros::Time& time);
00101     void publishPose(const tf::Transform& transform);
00102 
00103     void rosToPMScan(const sensor_msgs::LaserScan& scan, 
00104                      const tf::Transform& change,
00105                            PMScan* pmScan);
00106     void pose2DToTf(const geometry_msgs::Pose2D& pose, tf::Transform& t);
00107     void tfToPose2D(const tf::Transform& t, geometry_msgs::Pose2D& pose);
00108     void getCurrentEstimatedPose(tf::Transform& worldToBase, 
00109                                  const sensor_msgs::LaserScan& scanMsg);
00110 
00111   public:
00112 
00113     PSMNode();
00114     virtual ~PSMNode();
00115 };
00116 
00117 #endif // POLAR_SCAN_MATCHER_PSM_NODE_H


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