VisualSensors.h

#ifndef CONTROL_VISUAL_SENSORS_H
#define CONTROL_VISUAL_SENSORS_H

#include "Sensor.h"
#include <KrisLibrary/camera/viewport.h>
#include <KrisLibrary/math3d/primitives.h>
#include <KrisLibrary/GLdraw/GLRenderToImage.h>
using namespace Math3D;

class RobotWorld;
class Robot;

class LaserRangeSensor : public SensorBase
{
 public:
  LaserRangeSensor();
  virtual ~LaserRangeSensor() {}
  virtual const char* Type() const { return "LaserRangeSensor"; }
  virtual void Simulate(ControlledRobotSimulator* robot,WorldSimulation* sim);
  virtual void SimulateKinematic(Robot& robot,RobotWorld& world);
  virtual void Advance(double dt);
  virtual void Reset();
  virtual void MeasurementNames(vector<string>& names) const;
  virtual void GetMeasurements(vector<double>& values) const;
  virtual void SetMeasurements(const vector<double>& values);
  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);

  int link;
  RigidTransform Tsensor; 
  int measurementCount;   
  double depthResolution; 
  double depthMinimum,depthMaximum;    
  double depthVarianceLinear,depthVarianceConstant;
  enum { SweepSinusoid, SweepTriangular, SweepSawtooth};
  Real xSweepMagnitude,xSweepPeriod,xSweepPhase;
  int xSweepType;
  Real ySweepMagnitude,ySweepPeriod,ySweepPhase;
  int ySweepType; 

  //simulated depth readings
  vector<double> depthReadings;
  //internal state
  Real last_dt,last_t;
};


class CameraSensor : public SensorBase
{
 public:
  CameraSensor();
  virtual ~CameraSensor();
  virtual const char* Type() const { return "CameraSensor"; }
  virtual void Simulate(ControlledRobotSimulator* robot,WorldSimulation* sim);
  virtual void SimulateKinematic(Robot& robot,RobotWorld& world);
  virtual void Reset();
  virtual void MeasurementNames(vector<string>& names) const;
  virtual void GetMeasurements(vector<double>& values) const;
  virtual void SetMeasurements(const vector<double>& values);
  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);
  void GetViewport(Camera::Viewport& view) const;
  void SetViewport(const Camera::Viewport& view);

  int link;
  RigidTransform Tsensor; 
  bool rgb,depth;  
  int xres,yres;  
  double xfov,yfov; 
  double zmin,zmax;  
  int zresolution;  
  double zvarianceLinear;  
  double zvarianceConstant;  

  //internal: used for OpenGL rendering / buffers
  bool useGLFramebuffers; 
  GLDraw::GLRenderToImage renderer;
  vector<unsigned char> pixels;
  vector<float> floats;
  vector<double> measurements;
};

#endif