pfuclt_particles.h

Go to the documentation of this file.

#ifndef PARTICLES_H
#define PARTICLES_H

#include <ros/ros.h>
#include <pfuclt_omni_dataset/pfuclt_aux.h>

#include <vector>
#include <algorithm>
#include <iostream>
#include <sstream>
#include <boost/random.hpp>
#include <boost/thread/mutex.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include <pfuclt_omni_dataset/particle.h>
#include <pfuclt_omni_dataset/particles.h>

#include <dynamic_reconfigure/server.h>
#include <pfuclt_omni_dataset/DynamicConfig.h>

#define NUM_ALPHAS 4

// ideally later this will be a parameter, when it makes sense to
#define STATES_PER_TARGET 3

// offsets
#define O_X (0)
#define O_Y (1)
#define O_THETA (2)
//#define O_TARGET (nRobots_ * nStatesPerRobot_)
#define O_TX (0)
#define O_TY (1)
#define O_TZ (2)
//#define O_WEIGHT (nSubParticleSets_ - 1)

// target motion model and estimator
#define TARGET_RAND_MEAN 0
#define TARGET_RAND_STDDEV_LOST 500.0

// concerning time
#define TARGET_ITERATION_TIME_DEFAULT 0.0333
#define TARGET_ITERATION_TIME_MAX (1)

// others
#define MIN_WEIGHTSUM 1e-10

//#define MORE_DEBUG true

namespace pfuclt_omni_dataset
{
typedef float pdata_t;

typedef double (*estimatorFunc)(const std::vector<double>&,
                                const std::vector<double>&);

typedef struct odometry_s
{
  double x, y, theta;
} Odometry;

typedef struct landmarkObs_s
{
  bool found;
  double x, y;
  double d, phi;
  double covDD, covPP, covXX, covYY;
  landmarkObs_s() { found = false; }
} LandmarkObservation;

typedef struct targetObs_s
{
  bool found;
  double x, y, z;
  double d, phi, r;
  double covDD, covPP, covXX, covYY;

  targetObs_s() { found = false; }
} TargetObservation;

// Apply concept of subparticles (the particle set for each dimension)
typedef std::vector<pdata_t> subparticles_t;
typedef std::vector<subparticles_t> particles_t;

// This will be the generator use for randomizing
typedef boost::random::mt19937 RNGType;

class ParticleFilter
{
private:
  boost::mutex mutex_;
  dynamic_reconfigure::Server<DynamicConfig>
      dynamicServer_;

protected:
  struct dynamicVariables_s
  {
    bool firstCallback;
    int nParticles;
    double resamplingPercentageToKeep;
    double targetRandStddev;
    double oldTargetRandSTddev;
    std::vector<std::vector<float> > alpha;

    dynamicVariables_s(ros::NodeHandle& nh, const uint nRobots);

    void fill_alpha(const uint robot, const std::string& str);

  } dynamicVariables_;

  struct State
  {
    uint nStatesPerRobot;
    uint nRobots;

    typedef struct robotState_s
    {
      std::vector<pdata_t> pose;
      pdata_t conf;

      robotState_s(uint poseSize) : pose(poseSize, 0.0), conf(0.0) {}
    } RobotState;
    std::vector<RobotState> robots;

    struct targetState_s
    {
      std::vector<pdata_t> pos;
      bool seen;

      targetState_s()
          : pos(STATES_PER_TARGET, 0.0), seen(false)
      {
      }
    } target;

    State(const uint nStatesPerRobot, const uint nRobots)
        : nStatesPerRobot(nStatesPerRobot), nRobots(nRobots)
    {
      // Create and initialize the robots vector
      for (uint r = 0; r < nRobots; ++r)
        robots.push_back(robotState_s(nStatesPerRobot));
    }
  };

public:
  struct PFinitData
  {
    ros::NodeHandle& nh;
    const uint mainRobotID, nTargets, statesPerRobot, nRobots, nLandmarks;
    const std::vector<bool>& robotsUsed;
    const std::vector<Landmark>& landmarksMap;

    PFinitData(ros::NodeHandle& nh, const uint mainRobotID, const uint nTargets,
               const uint statesPerRobot, const uint nRobots,
               const uint nLandmarks, const std::vector<bool>& robotsUsed,
               const std::vector<Landmark>& landmarksMap)
        : nh(nh), mainRobotID(mainRobotID), nTargets(nTargets),
          statesPerRobot(statesPerRobot), nRobots(nRobots),
          nLandmarks(nLandmarks), robotsUsed(robotsUsed),
          landmarksMap(landmarksMap)
    {
    }
  };

protected:
  ros::NodeHandle& nh_;
  uint nParticles_;
  const uint mainRobotID_;
  const uint nTargets_;
  const uint nStatesPerRobot_;
  const uint nRobots_;
  const uint nSubParticleSets_;
  const uint nLandmarks_;
  particles_t particles_;
  particles_t weightComponents_;
  RNGType seed_;
  bool initialized_;
  const std::vector<Landmark>& landmarksMap_;
  const std::vector<bool>& robotsUsed_;
  std::vector<std::vector<LandmarkObservation> > bufLandmarkObservations_;
  std::vector<TargetObservation> bufTargetObservations_;
  TimeEval targetIterationTime_, odometryTime_;
  ros::WallTime iterationEvalTime_;
  ros::WallDuration deltaIteration_, maxDeltaIteration_;
  ros::WallDuration durationSum;
  uint16_t numberIterations;
  struct State state_;
  ros::Time latestObservationTime_, savedLatestObservationTime_;
  bool converged_;

  inline void copyParticle(particles_t& p_To, particles_t& p_From, uint i_To,
                           uint i_From)
  {
    copyParticle(p_To, p_From, i_To, i_From, 0, p_To.size() - 1);
  }

  inline void copyParticle(particles_t& p_To, particles_t& p_From, uint i_To,
                           uint i_From, uint subFirst, uint subLast)
  {
    for (uint k = subFirst; k <= subLast; ++k)
      p_To[k][i_To] = p_From[k][i_From];
  }

  inline void resetWeights(pdata_t val)
  {
    particles_[O_WEIGHT].assign(particles_[O_WEIGHT].size(), val);
  }

  void spreadTargetParticlesSphere(float particlesRatio, pdata_t center[3],
                                   float radius);

  void predictTarget();

  void fuseRobots();

  void fuseTarget();

  void modifiedMultinomialResampler(uint startAt);

  void resample();

  void estimate();

  virtual void nextIteration() {}

  virtual void resize_particles(const uint n)
  {
    size_t old_size = particles_[0].size();

    // Resize weightComponents
    for (uint r = 0; r < weightComponents_.size(); ++r)
      weightComponents_[r].resize(n);

    // Resize particles
    for (uint s = 0; s < particles_.size(); ++s)
      particles_[s].resize(n);

    // If n is lower than old_size, the last particles are removed - the ones
    // with the most weight are kept
    // But if n is higher, it's better to resample
    if (n > old_size)
      resample();
  }

public:
  boost::shared_ptr<std::ostringstream> iteration_oss;
  uint O_TARGET, O_WEIGHT;

  void dynamicReconfigureCallback(pfuclt_omni_dataset::DynamicConfig&);

  ParticleFilter(struct PFinitData& data);

  void updateTargetIterationTime(ros::Time tRos)
  {
    targetIterationTime_.updateTime(tRos);

    if (fabs(targetIterationTime_.diff) > TARGET_ITERATION_TIME_MAX)
    {
      // Something is wrong, set to default iteration time
      targetIterationTime_.diff = TARGET_ITERATION_TIME_DEFAULT;
    }
    ROS_DEBUG("Target tracking iteration time: %f", targetIterationTime_.diff);
  }

  ParticleFilter* getPFReference() { return this; }

  void printWeights(std::string pre);

  void assign(const pdata_t value);

  void assign(const pdata_t value, const uint index);

  inline subparticles_t& operator[](int index) { return particles_[index]; }

  inline const subparticles_t& operator[](int index) const
  {
    return particles_[index];
  }

  void init();

  void init(const std::vector<double>& customRandInit,
            const std::vector<double>& customPosInit);

  void predict(const uint robotNumber, const Odometry odom,
               const ros::Time stamp);

  bool isInitialized() { return initialized_; }

  std::size_t size() { return nSubParticleSets_; }

  inline void saveLandmarkObservation(const uint robotNumber,
                                      const uint landmarkNumber,
                                      const LandmarkObservation obs,
                                      ros::Time stamp)
  {
    bufLandmarkObservations_[robotNumber][landmarkNumber] = obs;
    latestObservationTime_ = stamp;
  }

  inline void saveLandmarkObservation(const uint robotNumber,
                                      const uint landmarkNumber,
                                      const bool found)
  {
    bufLandmarkObservations_[robotNumber][landmarkNumber].found = found;
  }

  void saveAllLandmarkMeasurementsDone(const uint robotNumber);

  inline void saveTargetObservation(const uint robotNumber,
                                    const TargetObservation obs,
                                    ros::Time stamp)
  {
    bufTargetObservations_[robotNumber] = obs;

    // If previously target not seen and now is found
    if (obs.found && !state_.target.seen)
    {
      // Update to target seen
      state_.target.seen = true;

      // Observation to global frame
      const ParticleFilter::State::RobotState& rs = state_.robots[robotNumber];
      pdata_t ballGlobal[3];
      ballGlobal[O_TX] = rs.pose[O_X] + obs.x * cos(rs.pose[O_THETA]) -
                         obs.y * sin(rs.pose[O_THETA]);
      ballGlobal[O_TY] = rs.pose[O_Y] + obs.x * sin(rs.pose[O_THETA]) +
                         obs.y * cos(rs.pose[O_THETA]);
      ballGlobal[O_TZ] = obs.z;

      // Spread 50% of particles around ballGlobal in a sphere with 1.0 meter
      // radius
      spreadTargetParticlesSphere(0.5, ballGlobal, 1.0);
    }
  }

  inline void saveTargetObservation(const uint robotNumber, const bool found)
  {
    bufTargetObservations_[robotNumber].found = found;
  }

  void saveAllTargetMeasurementsDone(const uint robotNumber);
};

// end of namespace pfuclt_omni_dataset
}
#endif // PARTICLES_H