HumanoidLocalization.h
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  * 6D localization for humanoid robots
00006  *
00007  * Copyright 2009-2012 Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/humanoid_localization
00009  *
00010  *
00011  * This program is free software: you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation, version 3.
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 
00024 #ifndef HUMANOID_LOCALIZATION_HUMANOIDLOCALIZATION_H_
00025 #define HUMANOID_LOCALIZATION_HUMANOIDLOCALIZATION_H_
00026 
00027 #include <ctime>
00028 #include <boost/shared_ptr.hpp>
00029 
00030 #include <ros/ros.h>
00031 #include <tf/transform_datatypes.h>
00032 #include <tf/transform_listener.h>
00033 #include <tf/transform_broadcaster.h>
00034 #include <tf/message_filter.h>
00035 #include <message_filters/subscriber.h>
00036 #include <std_msgs/Float32.h>
00037 #include <std_msgs/Bool.h>
00038 #include <std_srvs/Empty.h>
00039 #include <sensor_msgs/LaserScan.h>
00040 #include <sensor_msgs/PointCloud2.h>
00041 
00042 #include <pcl_ros/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/ros/conversions.h>
00045 #include <pcl_ros/transforms.h>
00046 #include <pcl/sample_consensus/method_types.h>
00047 #include <pcl/sample_consensus/model_types.h>
00048 #include <pcl/segmentation/sac_segmentation.h>
00049 #include <pcl/io/pcd_io.h>
00050 #include <pcl/filters/extract_indices.h>
00051 #include <pcl/filters/passthrough.h>
00052 
00053 
00054 #include <geometry_msgs/Pose.h>
00055 #include <geometry_msgs/PoseArray.h>
00056 #include <geometry_msgs/PoseWithCovariance.h>
00057 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00058 
00059 #include <humanoid_localization/humanoid_localization_defs.h>
00060 #include <humanoid_localization/MotionModel.h>
00061 #include <humanoid_localization/ObservationModel.h>
00062 #include <humanoid_localization/RaycastingModel.h>
00063 #ifndef SKIP_ENDPOINT_MODEL
00064   #include <humanoid_localization/EndpointModel.h>
00065 #endif
00066 
00067 
00068 #include <octomap/octomap.h>
00069 #include <sensor_msgs/Imu.h>
00070 #include <boost/circular_buffer.hpp>
00071 
00072 namespace humanoid_localization{
00073 
00074 static inline void getRP(const geometry_msgs::Quaternion& msg_q, double& roll, double& pitch){
00075   tf::Quaternion bt_q;
00076   tf::quaternionMsgToTF(msg_q, bt_q);
00077   double useless_yaw;
00078   tf::Matrix3x3(bt_q).getRPY(roll, pitch, useless_yaw);
00079 
00080   if (std::abs(useless_yaw) > 0.00001)
00081     ROS_WARN("Non-zero yaw in IMU quaterion is ignored");
00082 }
00083 
00084 class HumanoidLocalization {
00085 public:
00086     // some typedefs
00087     //typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00088 
00089 public:
00090   HumanoidLocalization(unsigned randomSeed);
00091   virtual ~HumanoidLocalization();
00092   virtual void laserCallback(const sensor_msgs::LaserScanConstPtr& msg);
00093   virtual void pointCloudCallback(const PointCloud::ConstPtr& msg);
00094   void initPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
00095   bool globalLocalizationCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00096   void pauseLocalizationCallback(const std_msgs::BoolConstPtr& msg);
00098   bool pauseLocalizationSrvCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00100   bool resumeLocalizationSrvCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00101   void imuCallback(const sensor_msgs::ImuConstPtr& msg);
00102 
00109   void resample(unsigned numParticles = 0);
00111   unsigned getBestParticleIdx() const;
00113   tf::Pose getParticlePose(unsigned particleIdx) const;
00115   tf::Pose getBestParticlePose() const;
00117   tf::Pose getMeanParticlePose() const;
00118 
00120   void initGlobal();
00121 
00122   // needed for pointcloud callback (from OctomapServer)
00123   static void filterGroundPlane(const PointCloud& pc, PointCloud& ground, PointCloud& nonground, double groundFilterDistance, double groundFilterAngle, double groundFilterPlaneDistance);
00124 
00125 protected:
00131   void reset();
00132 
00133   // converts particles into PoseArray and publishes them for visualization
00134   void publishPoseEstimate(const ros::Time& time, bool publish_eval);
00135 
00136 
00142   void normalizeWeights();
00143 
00145   double getCumParticleWeight() const;
00146 
00152   double nEff() const;
00153 
00157   void toLogForm();
00158 
00167   bool getImuMsg(const ros::Time& stamp, ros::Time& imuStamp, double& angleX, double& angleY) const;
00168 
00174   void prepareLaserPointCloud(const sensor_msgs::LaserScanConstPtr& laser, PointCloud& pc, std::vector<float>& ranges) const;
00175 
00181   void prepareGeneralPointCloud(const PointCloud::ConstPtr & msg, PointCloud& pc, std::vector<float>& ranges) const;
00182   int filterUniform( const PointCloud & cloud_in, PointCloud & cloud_out, int numSamples) const;
00183   
00184   void voxelGridSampling(const PointCloud & pc, pcl::PointCloud<int> & sampledIndices, double searchRadius) const;
00185   
00186   bool isAboveMotionThreshold(const tf::Pose& odomTransform);
00187   
00188   bool localizeWithMeasurement(const PointCloud& pc_filtered, const std::vector<float>& ranges, double max_range);
00189 
00190   void constrainMotion(const tf::Pose& odomPose);
00191 
00192   void timerCallback(const ros::TimerEvent & e);
00193 
00194   unsigned computeBeamStep(unsigned numBeams) const;
00195 
00200   void initZRP(double& z, double& roll, double& pitch);
00201 
00202   bool lookupPoseHeight(const ros::Time& t, double& poseHeight) const;
00203 
00204   EngineT m_rngEngine;
00206   NormalGeneratorT m_rngNormal;
00208   UniformGeneratorT m_rngUniform;
00209   boost::shared_ptr<MotionModel> m_motionModel;
00210   boost::shared_ptr<ObservationModel> m_observationModel;
00211   boost::shared_ptr<MapModel> m_mapModel;
00212 
00213   ros::NodeHandle m_nh, m_privateNh;
00214   ros::Subscriber m_pauseIntegrationSub;
00215 
00216   message_filters::Subscriber<sensor_msgs::LaserScan>* m_laserSub;
00217   tf::MessageFilter<sensor_msgs::LaserScan>* m_laserFilter;
00218   message_filters::Subscriber< PointCloud >* m_pointCloudSub;
00219   tf::MessageFilter< PointCloud  >* m_pointCloudFilter;
00220   message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseSub;
00221   tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseFilter;
00222 
00223   ros::Publisher m_posePub, m_poseEvalPub, m_poseOdomPub, m_poseTruePub,
00224                  m_poseArrayPub, m_bestPosePub, m_nEffPub,
00225                  m_filteredPointCloudPub;
00226   ros::Subscriber m_imuSub;
00227   ros::ServiceServer m_globalLocSrv, m_pauseLocSrv, m_resumeLocSrv;
00228   tf::TransformListener m_tfListener;
00229   tf::TransformBroadcaster m_tfBroadcaster;
00230   ros::Timer m_timer;
00231 
00232   std::string m_odomFrameId;
00233   std::string m_targetFrameId;
00234   std::string m_baseFrameId;
00235   std::string m_baseFootprintId;
00236   std::string m_globalFrameId;
00237 
00238   bool m_useRaycasting;
00239   bool m_initFromTruepose;
00240   int m_numParticles;
00241   double m_sensorSampleDist;
00242 
00243   double m_nEffFactor;
00244   double m_minParticleWeight;
00245   Vector6d m_initPose;  // fixed init. pose (from params)
00246   Vector6d m_initNoiseStd; // Std.dev for init. pose
00247   bool m_initPoseRealZRP; // override z, roll, pitch with real values from robot
00248 
00249   double m_filterMaxRange;
00250   double m_filterMinRange;
00251 
00252 
00253   Particles m_particles;
00254   int m_bestParticleIdx;
00255   tf::Pose m_odomPose; // incrementally added odometry pose (=dead reckoning)
00256   geometry_msgs::PoseArray m_poseArray; // particles as PoseArray (preallocated)
00257   boost::circular_buffer<sensor_msgs::Imu> m_lastIMUMsgBuffer;
00258 
00259   bool m_bestParticleAsMean;
00260   bool m_receivedSensorData;
00261   bool m_initialized;
00262   bool m_initGlobal;
00263   bool m_paused;
00264   bool m_syncedTruepose;
00265 
00266   double m_observationThresholdTrans;
00267   double m_observationThresholdRot;
00268   double m_observationThresholdHeadYawRot;
00269   double m_observationThresholdHeadPitchRot;
00270   double m_temporalSamplingRange;
00271   double m_transformTolerance;
00272   ros::Time m_lastLaserTime;
00273   ros::Time m_lastPointCloudTime;
00274 
00275 
00276   // PointCloud parameters
00277 
00278   bool m_groundFilterPointCloud;
00279   double m_groundFilterDistance;
00280   double m_groundFilterAngle;
00281   double m_groundFilterPlaneDistance;
00282   double m_sensorSampleDistGroundFactor;
00283   int m_numFloorPoints;
00284   int m_numNonFloorPoints;
00285 
00286 
00288   tf::Pose m_lastLocalizedPose;
00289   tf::StampedTransform m_latest_transform;
00290 
00292   double m_headYawRotationLastScan;
00294   double m_headPitchRotationLastScan;
00295 
00296   bool m_useIMU;  
00297   bool m_constrainMotionZ; 
00298   bool m_constrainMotionRP; 
00299   
00300   // timer stuff
00301   bool m_useTimer;
00302   double m_timerPeriod;
00303 };
00304 }
00305 
00306 #endif


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Mon Oct 6 2014 00:41:01