Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00014 #ifndef HRPMODEL_SENSOR_H_INCLUDED
00015 #define HRPMODEL_SENSOR_H_INCLUDED
00016
00017 #include <string>
00018 #include <iostream>
00019 #include <vector>
00020 #include <hrpUtil/Eigen3d.h>
00021 #include "Config.h"
00022
00023 namespace hrp {
00024
00025 class Link;
00026
00027 class HRPMODEL_API Sensor
00028 {
00029 public:
00030
00031 enum SensorType {
00032 COMMON = 0,
00033 FORCE,
00034 RATE_GYRO,
00035 ACCELERATION,
00036 PRESSURE,
00037 PHOTO_INTERRUPTER,
00038 VISION,
00039 TORQUE,
00040 RANGE,
00041 NUM_SENSOR_TYPES
00042 };
00043
00044 static const int TYPE = COMMON;
00045
00046 Sensor();
00047 virtual ~Sensor();
00048
00049 static Sensor* create(int type);
00050 static void destroy(Sensor* sensor);
00051
00052 virtual void operator=(const Sensor& org);
00053
00054 virtual void clear();
00055
00056 std::string name;
00057 int type;
00058 int id;
00059 Link* link;
00060 Matrix33 localR;
00061 Vector3 localPos;
00062
00063 virtual void putInformation(std::ostream& os);
00064
00065 };
00066
00067
00068 class HRPMODEL_API ForceSensor : public Sensor
00069 {
00070 public:
00071 static const int TYPE = FORCE;
00072
00073 ForceSensor();
00074 Vector3 f;
00075 Vector3 tau;
00076
00077 virtual void clear();
00078 virtual void putInformation(std::ostream& os);
00079 };
00080
00081
00082 class HRPMODEL_API RateGyroSensor : public Sensor
00083 {
00084 public:
00085 static const int TYPE = RATE_GYRO;
00086
00087 RateGyroSensor();
00088 Vector3 w;
00089
00090 virtual void clear();
00091 virtual void putInformation(std::ostream& os);
00092 };
00093
00094
00095 class HRPMODEL_API AccelSensor : public Sensor
00096 {
00097 public:
00098 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00099
00100 static const int TYPE = ACCELERATION;
00101
00102 AccelSensor();
00103
00104 Vector3 dv;
00105
00106 virtual void clear();
00107 virtual void putInformation(std::ostream& os);
00108
00109
00110 typedef Eigen::Vector2d vector2;
00111 vector2 x[3];
00112 bool isFirstUpdate;
00113 };
00114
00115 class HRPMODEL_API RangeSensor : public Sensor
00116 {
00117 public:
00118 static const int TYPE = RANGE;
00119
00120 RangeSensor();
00121
00122 double scanAngle, scanStep, scanRate, maxDistance;
00123 std::vector<double> distances;
00124 double nextUpdateTime;
00125 bool isUpdated, isEnabled;
00126 };
00127
00128 #ifdef far
00129 #undef far
00130 #endif
00131 #ifdef near
00132 #undef near
00133 #endif
00134
00135 class HRPMODEL_API VisionSensor : public Sensor
00136 {
00137 public:
00138 typedef enum {NONE, COLOR, MONO, DEPTH, COLOR_DEPTH, MONO_DEPTH} ImageType;
00139 static const int TYPE = VISION;
00140
00141 VisionSensor();
00142 int width, height;
00143 double far, near, fovy, frameRate;
00144 ImageType imageType;
00145 std::vector<unsigned char> image;
00146 std::vector<unsigned char> depth;
00147 double nextUpdateTime;
00148 bool isUpdated, isEnabled;
00149 };
00150 };
00151
00152
00153 #endif