Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00087
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
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
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;
00246 Vector6d m_initNoiseStd;
00247 bool m_initPoseRealZRP;
00248
00249 double m_filterMaxRange;
00250 double m_filterMinRange;
00251
00252
00253 Particles m_particles;
00254 int m_bestParticleIdx;
00255 tf::Pose m_odomPose;
00256 geometry_msgs::PoseArray m_poseArray;
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
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
00301 bool m_useTimer;
00302 double m_timerPeriod;
00303 };
00304 }
00305
00306 #endif