HumanoidLocalization.h

Go to the documentation of this file.

// SVN $HeadURL$
// SVN $Id$

/*
 * 6D localization for humanoid robots
 *
 * Copyright 2009-2012 Armin Hornung, University of Freiburg
 * http://www.ros.org/wiki/humanoid_localization
 *
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef HUMANOID_LOCALIZATION_HUMANOIDLOCALIZATION_H_
#define HUMANOID_LOCALIZATION_HUMANOIDLOCALIZATION_H_

#include <ctime>
#include <boost/shared_ptr.hpp>

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Bool.h>
#include <std_srvs/Empty.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>


#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <humanoid_localization/humanoid_localization_defs.h>
#include <humanoid_localization/MotionModel.h>
#include <humanoid_localization/ObservationModel.h>
#include <humanoid_localization/RaycastingModel.h>
#ifndef SKIP_ENDPOINT_MODEL
  #include <humanoid_localization/EndpointModel.h>
#endif


#include <octomap/octomap.h>
#include <sensor_msgs/Imu.h>
#include <boost/circular_buffer.hpp>

namespace humanoid_localization{

static inline void getRP(const geometry_msgs::Quaternion& msg_q, double& roll, double& pitch){
  tf::Quaternion bt_q;
  tf::quaternionMsgToTF(msg_q, bt_q);
  double useless_yaw;
  tf::Matrix3x3(bt_q).getRPY(roll, pitch, useless_yaw);

  if (std::abs(useless_yaw) > 0.00001)
    ROS_WARN("Non-zero yaw in IMU quaterion is ignored");
}

class HumanoidLocalization {
public:
    // some typedefs
    //typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

public:
  HumanoidLocalization(unsigned randomSeed);
  virtual ~HumanoidLocalization();
  virtual void laserCallback(const sensor_msgs::LaserScanConstPtr& msg);
  virtual void pointCloudCallback(const PointCloud::ConstPtr& msg);
  void initPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
  bool globalLocalizationCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
  void pauseLocalizationCallback(const std_msgs::BoolConstPtr& msg);
  bool pauseLocalizationSrvCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
  bool resumeLocalizationSrvCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
  void imuCallback(const sensor_msgs::ImuConstPtr& msg);

  void resample(unsigned numParticles = 0);
  unsigned getBestParticleIdx() const;
  tf::Pose getParticlePose(unsigned particleIdx) const;
  tf::Pose getBestParticlePose() const;
  tf::Pose getMeanParticlePose() const;

  void initGlobal();

  // needed for pointcloud callback (from OctomapServer)
  static void filterGroundPlane(const PointCloud& pc, PointCloud& ground, PointCloud& nonground, double groundFilterDistance, double groundFilterAngle, double groundFilterPlaneDistance);

protected:
  void reset();

  // converts particles into PoseArray and publishes them for visualization
  void publishPoseEstimate(const ros::Time& time, bool publish_eval);


  void normalizeWeights();

  double getCumParticleWeight() const;

  double nEff() const;

  void toLogForm();

  bool getImuMsg(const ros::Time& stamp, ros::Time& imuStamp, double& angleX, double& angleY) const;

  void prepareLaserPointCloud(const sensor_msgs::LaserScanConstPtr& laser, PointCloud& pc, std::vector<float>& ranges) const;

  void prepareGeneralPointCloud(const PointCloud::ConstPtr & msg, PointCloud& pc, std::vector<float>& ranges) const;
  int filterUniform( const PointCloud & cloud_in, PointCloud & cloud_out, int numSamples) const;
  
  void voxelGridSampling(const PointCloud & pc, pcl::PointCloud<int> & sampledIndices, double searchRadius) const;
  
  bool isAboveMotionThreshold(const tf::Pose& odomTransform);
  
  bool localizeWithMeasurement(const PointCloud& pc_filtered, const std::vector<float>& ranges, double max_range);

  void constrainMotion(const tf::Pose& odomPose);

  void timerCallback(const ros::TimerEvent & e);

  unsigned computeBeamStep(unsigned numBeams) const;

  void initZRP(double& z, double& roll, double& pitch);

  bool lookupPoseHeight(const ros::Time& t, double& poseHeight) const;

  EngineT m_rngEngine;
  NormalGeneratorT m_rngNormal;
  UniformGeneratorT m_rngUniform;
  boost::shared_ptr<MotionModel> m_motionModel;
  boost::shared_ptr<ObservationModel> m_observationModel;
  boost::shared_ptr<MapModel> m_mapModel;

  ros::NodeHandle m_nh, m_privateNh;
  ros::Subscriber m_pauseIntegrationSub;

  message_filters::Subscriber<sensor_msgs::LaserScan>* m_laserSub;
  tf::MessageFilter<sensor_msgs::LaserScan>* m_laserFilter;
  message_filters::Subscriber< PointCloud >* m_pointCloudSub;
  tf::MessageFilter< PointCloud  >* m_pointCloudFilter;
  message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseSub;
  tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* m_initPoseFilter;

  ros::Publisher m_posePub, m_poseEvalPub, m_poseOdomPub, m_poseTruePub,
                 m_poseArrayPub, m_bestPosePub, m_nEffPub,
                 m_filteredPointCloudPub;
  ros::Subscriber m_imuSub;
  ros::ServiceServer m_globalLocSrv, m_pauseLocSrv, m_resumeLocSrv;
  tf::TransformListener m_tfListener;
  tf::TransformBroadcaster m_tfBroadcaster;
  ros::Timer m_timer;

  std::string m_odomFrameId;
  std::string m_targetFrameId;
  std::string m_baseFrameId;
  std::string m_baseFootprintId;
  std::string m_globalFrameId;

  bool m_useRaycasting;
  bool m_initFromTruepose;
  int m_numParticles;
  double m_sensorSampleDist;

  double m_nEffFactor;
  double m_minParticleWeight;
  Vector6d m_initPose;  // fixed init. pose (from params)
  Vector6d m_initNoiseStd; // Std.dev for init. pose
  bool m_initPoseRealZRP; // override z, roll, pitch with real values from robot

  double m_filterMaxRange;
  double m_filterMinRange;


  Particles m_particles;
  int m_bestParticleIdx;
  tf::Pose m_odomPose; // incrementally added odometry pose (=dead reckoning)
  geometry_msgs::PoseArray m_poseArray; // particles as PoseArray (preallocated)
  boost::circular_buffer<sensor_msgs::Imu> m_lastIMUMsgBuffer;

  bool m_bestParticleAsMean;
  bool m_receivedSensorData;
  bool m_initialized;
  bool m_initGlobal;
  bool m_paused;
  bool m_syncedTruepose;

  double m_observationThresholdTrans;
  double m_observationThresholdRot;
  double m_observationThresholdHeadYawRot;
  double m_observationThresholdHeadPitchRot;
  double m_temporalSamplingRange;
  double m_transformTolerance;
  ros::Time m_lastLaserTime;
  ros::Time m_lastPointCloudTime;


  // PointCloud parameters

  bool m_groundFilterPointCloud;
  double m_groundFilterDistance;
  double m_groundFilterAngle;
  double m_groundFilterPlaneDistance;
  double m_sensorSampleDistGroundFactor;
  int m_numFloorPoints;
  int m_numNonFloorPoints;


  tf::Pose m_lastLocalizedPose;
  tf::StampedTransform m_latest_transform;

  double m_headYawRotationLastScan;
  double m_headPitchRotationLastScan;

  bool m_useIMU;  
  bool m_constrainMotionZ; 
  bool m_constrainMotionRP; 
  
  // timer stuff
  bool m_useTimer;
  double m_timerPeriod;
};
}

#endif