Sensor.h

#ifndef CONTROL_SENSORS_H
#define CONTROL_SENSORS_H

#include <KrisLibrary/math/vector.h>
#include <map>
#include <vector>
#include <memory>
#include <string>
#include <typeinfo>
using namespace std;

class Robot;
class RobotWorld;
class ControlledRobotSimulator;
class WorldSimulation;
class TiXmlElement;

class SensorBase
{
 public:
  SensorBase();
  virtual ~SensorBase() {}
  virtual const char* Type() const { return "SensorBase"; }
  virtual void Simulate(ControlledRobotSimulator* robot,WorldSimulation* sim) {}
  virtual void SimulateKinematic(Robot& robot,RobotWorld& world) {}
  virtual void Advance(double dt) {}
  virtual void Reset() {}
  virtual bool ReadState(File& f);
  virtual bool WriteState(File& f) const;
  virtual void MeasurementNames(vector<string>& names) const { names.resize(0); }
  virtual void GetMeasurements(vector<double>& values) const { values.resize(0); }
  virtual void SetMeasurements(const vector<double>& values) { }
  virtual void GetInternalState(vector<double>& state) const {  }
  virtual void SetInternalState(const vector<double>& state) { }
  virtual map<string,string> Settings() const;
  virtual bool GetSetting(const string& name,string& str) const;
  virtual bool SetSetting(const string& name,const string& str);
  virtual void DrawGL(const Robot& robot,const vector<double>& measurements) {}

  string name;
  double rate;
};




class RobotSensors
{
 public:
  void MakeDefault(Robot* robot);
  bool LoadSettings(const char* fn);
  bool SaveSettings(const char* fn);
  bool LoadSettings(TiXmlElement* in);
  void SaveSettings(TiXmlElement* out);
  bool LoadMeasurements(TiXmlElement* in);
  void SaveMeasurements(TiXmlElement* out);
  bool ReadState(File& f);
  bool WriteState(File& f) const;
  shared_ptr<SensorBase> GetNamedSensor(const string& name);
  template <class T>
  void GetTypedSensors(vector<T*>& sensors);
  template <class T>
  T* GetTypedSensor(int index=0);

  vector<shared_ptr<SensorBase> > sensors;
};



template <class T>
void RobotSensors::GetTypedSensors(vector<T*>& _sensors)
{
  _sensors.resize(0);
  for(size_t i=0;i<sensors.size();i++)
    if(typeid(T) == typeid(*sensors[i])) _sensors.push_back(dynamic_cast<T*>(sensors[i].get()));
}


template <class T>
T* RobotSensors::GetTypedSensor(int index)
{
  for(size_t i=0;i<sensors.size();i++) {
    if(typeid(T) == typeid(*sensors[i])) {
      if(index==0) return dynamic_cast<T*>(sensors[i].get());
      index--;
    }
  }
  return NULL;
}



//these macros will help you read in / write out settings
#define FILL_SENSOR_SETTING(res,membername) \
  { \
    stringstream ss;  \
    ss<<membername;   \
    res[#membername] = ss.str();  \
  }
#define GET_SENSOR_SETTING(membername) \
  if(name == #membername) { \
    stringstream ss;   \
    ss << membername;  \
    str = ss.str();    \
    return true;       \
  }
#define SET_SENSOR_SETTING(membername) \
  if(name == #membername) { \
    stringstream ss(str);   \
    ss >> membername;       \
    return bool(ss);              \
  }

#define FILL_ARRAY_SENSOR_SETTING(res,membername,count) \
  { \
    stringstream ss;                      \
    for(int _i=0;_i<count;_i++)             \
      ss<<membername[_i]<<" ";             \
    settings[#membername] = ss.str();     \
  }
#define GET_ARRAY_SENSOR_SETTING(membername,count)      \
  if(name == #membername) { \
    stringstream ss;   \
    for(int _i=0;_i<count;_i++)             \
      ss << membername[_i]<<" ";                  \
    str = ss.str();    \
    return true;       \
  }
#define SET_ARRAY_SENSOR_SETTING(membername,count)      \
  if(name == #membername) {     \
    stringstream ss(str);         \
    for(int _i=0;_i<count;_i++)     \
      ss >> membername[_i];       \
    return bool(ss);              \
  }

#define FILL_VECTOR_SENSOR_SETTING(res,membername) \
  { \
    stringstream ss;                      \
    for(size_t _i=0;_i<membername.size();_i++)             \
      ss<<membername[_i]<<" ";             \
    settings[#membername] = ss.str();     \
  }
#define GET_VECTOR_SENSOR_SETTING(membername)  \
  if(name == #membername) { \
    stringstream ss;   \
    for(size_t _i=0;_i<membername.size();_i++)            \
      ss << membername[_i]<<" ";      \
    str = ss.str();    \
    return true;       \
  }
#define SET_VECTOR_SENSOR_SETTING(membername)  \
  if(name == #membername) { \
    stringstream ss(str);   \
    membername.resize(0); \
    while(ss) {     \
      membername.resize(membername.size()+1); \
      ss >> membername.back();   \
      if(ss.fail()) { \
        membername.resize(membername.size()-1); \
        return true; \
      } \
      if(ss.bad()) return false; \
    } \
    return true;              \
  }

#endif