pfuclt_omni_dataset.h

Go to the documentation of this file.

// STL libraries
#include <vector>
#include <algorithm>
#include <cstdio>
#include <fstream>
#include <string>

// ROS message definitions
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <read_omni_dataset/BallData.h>
#include <read_omni_dataset/LRMLandmarksData.h>

// Eigen libraries
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

// Boost libraries
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/shared_ptr.hpp>

// Auxiliary libraries
#include <pfuclt_omni_dataset/pfuclt_aux.h>
#include <pfuclt_omni_dataset/pfuclt_particles.h>
#include <pfuclt_omni_dataset/pfuclt_publisher.h>

namespace pfuclt_omni_dataset
{

#define STATES_PER_ROBOT 3
#define HEURISTICS_THRESH_DEFAULT                                              \
  {                                                                            \
    2.5, 2.5, 2.5, 2.5, FLT_MAX, FLT_MAX, 3.5, 3.5, FLT_MAX, FLT_MAX           \
  }

// Forward declaration of classes
class Robot;

// useful typedefs
typedef boost::shared_ptr<Robot> Robot_ptr;

class RobotFactory
{

private:
  ros::NodeHandle& nh_;
  std::vector<Robot_ptr> robots_;

  bool areAllRobotsActive();

public:
  boost::shared_ptr<ParticleFilter> pf;

  RobotFactory(ros::NodeHandle& nh);

  void tryInitializeParticles();

  void initializeFixedLandmarks();
};

class Robot
{
protected:
  RobotFactory* parent_;
  ParticleFilter* pf_;
  bool started_;
  ros::Time timeStarted_;
  ros::Subscriber sOdom_, sBall_, sLandmark_;
  uint robotNumber_;
  Eigen::Isometry2d initPose_; // x y theta;

  void startNow();

public:
  Robot(ros::NodeHandle& nh, RobotFactory* parent,
        ParticleFilter* pf, uint robotNumber);

  void odometryCallback(const nav_msgs::Odometry::ConstPtr&);

  void targetCallback(const read_omni_dataset::BallData::ConstPtr&);

  void
  landmarkDataCallback(const read_omni_dataset::LRMLandmarksData::ConstPtr&);

  bool hasStarted() { return started_; }
};

// end of namespace pfuclt_omni_dataset
}