00001 /* 00002 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc. 00003 * All rights reserved. This program is made available under the terms of the 00004 * Eclipse Public License v1.0 which accompanies this distribution, and is 00005 * available at http://www.eclipse.org/legal/epl-v10.html 00006 * Contributors: 00007 * National Institute of Advanced Industrial Science and Technology (AIST) 00008 */ 00009 00010 00011 #include "Sensor.h" 00012 00013 #ifndef M_PI 00014 #define M_PI 3.14159265358979323846 00015 #endif 00016 00017 using namespace hrp; 00018 00019 00020 Sensor::Sensor() 00021 { 00022 type = COMMON; 00023 } 00024 00025 00026 Sensor* Sensor::create(int type) 00027 { 00028 Sensor* sensor; 00029 00030 switch(type){ 00031 00032 case FORCE: 00033 sensor = new ForceSensor(); 00034 break; 00035 00036 case RATE_GYRO: 00037 sensor = new RateGyroSensor(); 00038 break; 00039 00040 case ACCELERATION: 00041 sensor = new AccelSensor(); 00042 break; 00043 00044 case RANGE: 00045 sensor = new RangeSensor(); 00046 break; 00047 case VISION: 00048 sensor = new VisionSensor(); 00049 break; 00050 00051 case PRESSURE: 00052 case PHOTO_INTERRUPTER: 00053 case TORQUE: 00054 sensor = new Sensor(); 00055 break; 00056 default: 00057 sensor = 0; 00058 } 00059 00060 return sensor; 00061 } 00062 00063 00064 Sensor:: ~Sensor() 00065 { 00066 00067 } 00068 00069 00070 void Sensor::operator=(const Sensor& org) 00071 { 00072 name = org.name; 00073 type = org.type; 00074 id = org.id; 00075 localR = org.localR; 00076 localPos = org.localPos; 00077 } 00078 00079 00080 void Sensor::destroy(Sensor* sensor) 00081 { 00082 delete sensor; 00083 } 00084 00085 00086 void Sensor::clear() 00087 { 00088 00089 } 00090 00091 00092 void Sensor::putInformation(std::ostream &os) 00093 { 00094 os << "name = " << name << ", id = " << id << "\n"; 00095 os << "localAttitude = " << localR << ", localPos = " << localPos << std::endl; 00096 } 00097 00098 00099 ForceSensor::ForceSensor() 00100 { 00101 type = FORCE; 00102 } 00103 00104 00105 void ForceSensor::clear() 00106 { 00107 f.setZero(); 00108 tau.setZero(); 00109 } 00110 00111 00112 void ForceSensor::putInformation(std::ostream& os) 00113 { 00114 os << "Force Sensor\n"; 00115 Sensor::putInformation(os); 00116 os << "f = " << f << "tau = " << tau << std::endl; 00117 } 00118 00119 00120 RateGyroSensor::RateGyroSensor() 00121 { 00122 type = RATE_GYRO; 00123 } 00124 00125 00126 void RateGyroSensor::clear() 00127 { 00128 w.setZero(); 00129 } 00130 00131 00132 void RateGyroSensor::putInformation(std::ostream& os) 00133 { 00134 os << "Gyro\n"; 00135 Sensor::putInformation(os); 00136 os << "omega = " << w << std::endl; 00137 } 00138 00139 00140 AccelSensor::AccelSensor() 00141 { 00142 type = ACCELERATION; 00143 clear(); 00144 } 00145 00146 00147 void AccelSensor::clear() 00148 { 00149 dv(0) = 0.0; 00150 dv(1) = 0.0; 00151 dv(2) = 9.8; 00152 } 00153 00154 00155 void AccelSensor::putInformation(std::ostream& os) 00156 { 00157 os << "Acceleration Sensor\n"; 00158 Sensor::putInformation(os); 00159 os << "dv = " << dv << std::endl; 00160 } 00161 00162 RangeSensor::RangeSensor() 00163 { 00164 type = RANGE; 00165 scanAngle = M_PI; 00166 scanStep = 0.1; 00167 scanRate = 10; 00168 maxDistance = 10; 00169 nextUpdateTime = 0; 00170 isUpdated = false; 00171 isEnabled = false; 00172 } 00173 00174 VisionSensor::VisionSensor() 00175 { 00176 type = VISION; 00177 near = 0.01; 00178 far = 10.0; 00179 width = 320; 00180 height = 240; 00181 imageType = NONE; 00182 frameRate = 30.0; 00183 nextUpdateTime = 0; 00184 isUpdated = false; 00185 isEnabled = false; 00186 }