00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef HARDWARE_INTERFACE_H
00036 #define HARDWARE_INTERFACE_H
00037
00038 #include <string>
00039 #include <vector>
00040
00041 #include <ros/ros.h>
00042
00043 #include <geometry_msgs/Vector3.h>
00044 #include <geometry_msgs/Wrench.h>
00045
00046 namespace pr2_hardware_interface{
00047
00048
00049 class ActuatorState{
00050 public:
00051 ActuatorState() :
00052 timestamp_(0),
00053 device_id_(0),
00054 encoder_count_(0),
00055 position_(0),
00056 encoder_velocity_(0),
00057 velocity_(0),
00058 calibration_reading_(0),
00059 calibration_rising_edge_valid_(0),
00060 calibration_falling_edge_valid_(0),
00061 last_calibration_rising_edge_(0),
00062 last_calibration_falling_edge_(0),
00063 is_enabled_(0),
00064 halted_(0),
00065 last_commanded_current_(0),
00066 last_executed_current_(0),
00067 last_measured_current_(0),
00068 last_commanded_effort_(0),
00069 last_executed_effort_(0),
00070 last_measured_effort_(0),
00071 motor_voltage_(0),
00072 num_encoder_errors_(0),
00073 zero_offset_(0)
00074 {}
00075
00083 ros::Duration sample_timestamp_;
00084
00090 double timestamp_;
00091
00092 int device_id_;
00093
00094 int encoder_count_;
00095 double position_;
00096 double encoder_velocity_;
00097 double velocity_;
00098
00099 bool calibration_reading_;
00100 bool calibration_rising_edge_valid_;
00101 bool calibration_falling_edge_valid_;
00102 double last_calibration_rising_edge_;
00103 double last_calibration_falling_edge_;
00104
00105 bool is_enabled_;
00106 bool halted_;
00107
00108 double last_commanded_current_;
00109 double last_executed_current_;
00110 double last_measured_current_;
00111
00112 double last_commanded_effort_;
00113 double last_executed_effort_;
00114 double last_measured_effort_;
00115
00116 double max_effort_;
00117
00118 double motor_voltage_;
00119
00120 int num_encoder_errors_;
00121
00122 double zero_offset_;
00123 };
00124
00125 class ActuatorCommand
00126 {
00127 public:
00128 ActuatorCommand() :
00129 enable_(0), effort_(0)
00130 {}
00131 bool enable_;
00132 double effort_;
00133 };
00134
00147 class Actuator
00148 {
00149 public:
00150 Actuator() {};
00151 Actuator(std::string name) : name_(name) {}
00152 std::string name_;
00153 ActuatorState state_;
00154 ActuatorCommand command_;
00155 };
00156
00157 class PressureSensorCommand
00158 {
00159 };
00160
00161 class PressureSensorState
00162 {
00163 public:
00164 std::vector<uint16_t> data_;
00165 };
00166
00177 class PressureSensor
00178 {
00179 public:
00180 std::string name_;
00181 PressureSensorState state_;
00182 PressureSensorCommand command_;
00183 };
00184
00185 class AccelerometerCommand
00186 {
00187 public:
00188 enum {RANGE_2G=0, RANGE_4G=1, RANGE_8G=2};
00189 enum {BANDWIDTH_1500HZ=6, BANDWIDTH_750HZ=5, BANDWIDTH_375HZ=4, BANDWIDTH_190HZ=3, BANDWIDTH_100HZ=2, BANDWIDTH_50HZ=1, BANDWIDTH_25HZ=0};
00190 AccelerometerCommand() : range_(RANGE_2G), bandwidth_(BANDWIDTH_1500HZ) {}
00191 int range_;
00192 int bandwidth_;
00193 };
00194
00195 class AccelerometerState
00196 {
00197 public:
00198 std::string frame_id_;
00199 std::vector<geometry_msgs::Vector3> samples_;
00200 };
00201
00212 class Accelerometer
00213 {
00214 public:
00215 std::string name_;
00216 AccelerometerState state_;
00217 AccelerometerCommand command_;
00218 };
00219
00220 class ForceTorqueState
00221 {
00222 public:
00223 bool good_;
00224
00225 std::vector<geometry_msgs::Wrench> samples_;
00226 };
00227
00228 class ForceTorqueCommand
00229 {
00230 public:
00232 bool halt_on_error_;
00233 };
00234
00235 class ForceTorque
00236 {
00237 public:
00238 ForceTorqueState state_;
00239 ForceTorqueCommand command_;
00240 };
00241
00242
00243 class DigitalOutCommand
00244 {
00245 public:
00246 DigitalOutCommand() : data_(false) {}
00247 DigitalOutCommand(bool data) : data_(data) {}
00248 uint8_t data_;
00249 };
00250
00251 class DigitalOutState
00252 {
00253 public:
00254 uint8_t data_;
00255 };
00256
00267 class DigitalOut
00268 {
00269 public:
00270 std::string name_;
00271 DigitalOutState state_;
00272 DigitalOutCommand command_;
00273 };
00274
00275 class ProjectorCommand
00276 {
00277 public:
00278 ProjectorCommand(uint8_t &A, uint8_t &B, uint8_t &I, uint8_t &M, uint8_t &L0, uint8_t &L1) : enable_(0), current_(0), A_(A), B_(B), I_(I), M_(M), L0_(L0), L1_(L1) {}
00279 bool enable_;
00280 double current_;
00281 uint8_t &A_;
00282 uint8_t &B_;
00283 uint8_t &I_;
00284 uint8_t &M_;
00285 uint8_t &L0_;
00286 uint8_t &L1_;
00287 bool pulse_replicator_;
00288 };
00289
00290 class ProjectorState
00291 {
00292 public:
00293 ProjectorState(uint8_t &A, uint8_t &B, uint8_t &I, uint8_t &M, uint8_t &L0, uint8_t &L1) : A_(A), B_(B), I_(I), M_(M), L0_(L0), L1_(L1) {}
00294 bool enable_;
00295 double last_commanded_current_;
00296 double last_executed_current_;
00297 double last_measured_current_;
00298 double max_current_;
00299 uint8_t &A_;
00300 uint8_t &B_;
00301 uint8_t &I_;
00302 uint8_t &M_;
00303 uint8_t &L0_;
00304 uint8_t &L1_;
00305 bool pulse_replicator_;
00306
00307 bool output_;
00308 bool rising_timestamp_valid_;
00309 bool falling_timestamp_valid_;
00310
00311 uint32_t timestamp_us_;
00312 uint32_t rising_timestamp_us_;
00313 uint32_t falling_timestamp_us_;
00314 };
00315
00334 class Projector
00335 {
00336 public:
00337 Projector(DigitalOut &A, DigitalOut &B, DigitalOut &I, DigitalOut &M, DigitalOut &L0, DigitalOut &L1) : state_(A.state_.data_, B.state_.data_, I.state_.data_, M.state_.data_, L0.state_.data_, L1.state_.data_), command_(A.command_.data_, B.command_.data_, I.command_.data_, M.command_.data_, L0.command_.data_, L1.command_.data_) {}
00338 std::string name_;
00339 ProjectorState state_;
00340 ProjectorCommand command_;
00341 };
00342
00343 class AnalogInCommand
00344 {
00345 };
00346
00347 class AnalogInState
00348 {
00349 public:
00350 std::vector<double> state_;
00351 };
00352
00353 class AnalogIn
00354 {
00355 public:
00356 std::string name_;
00357 AnalogInState state_;
00358 AnalogInCommand command_;
00359 };
00360
00361 typedef std::map<std::string, Actuator*> ActuatorMap;
00362 typedef std::map<std::string, PressureSensor*> PressureSensorMap;
00363 typedef std::map<std::string, Accelerometer*> AccelerometerMap;
00364 typedef std::map<std::string, DigitalOut*> DigitalOutMap;
00365 typedef std::map<std::string, Projector*> ProjectorMap;
00366 typedef std::map<std::string, AnalogIn*> AnalogInMap;
00367
00392 class HardwareInterface
00393 {
00394 public:
00395 ActuatorMap actuators_;
00396 PressureSensorMap pressure_sensors_;
00397 AccelerometerMap accelerometers_;
00398 DigitalOutMap digital_outs_;
00399 ProjectorMap projectors_;
00400 AnalogInMap analog_ins_;
00406 Actuator* getActuator(const std::string &name) const {
00407 ActuatorMap::const_iterator it = actuators_.find(name);
00408 return it != actuators_.end() ? it->second : NULL;
00409 }
00410
00416 PressureSensor* getPressureSensor(const std::string &name) const {
00417 PressureSensorMap::const_iterator it = pressure_sensors_.find(name);
00418 return it != pressure_sensors_.end() ? it->second : NULL;
00419 }
00420
00426 Accelerometer* getAccelerometer(const std::string &name) const {
00427 AccelerometerMap::const_iterator it = accelerometers_.find(name);
00428 return it != accelerometers_.end() ? it->second : NULL;
00429 }
00430
00436 DigitalOut* getDigitalOut(const std::string &name) const {
00437 DigitalOutMap::const_iterator it = digital_outs_.find(name);
00438 return it != digital_outs_.end() ? it->second : NULL;
00439 }
00440
00446 Projector* getProjector(const std::string &name) const {
00447 ProjectorMap::const_iterator it = projectors_.find(name);
00448 return it != projectors_.end() ? it->second : NULL;
00449 }
00450
00456 AnalogIn* getAnalogIn(const std::string &name) const {
00457 AnalogInMap::const_iterator it = analog_ins_.find(name);
00458 return it != analog_ins_.end() ? it->second : NULL;
00459 }
00460
00466 bool addActuator(Actuator *actuator) {
00467 std::pair<ActuatorMap::iterator, bool> p;
00468 p = actuators_.insert(ActuatorMap::value_type(actuator->name_, actuator));
00469 return p.second;
00470 }
00471
00477 bool addPressureSensor(PressureSensor *sensor) {
00478 std::pair<PressureSensorMap::iterator, bool> p;
00479 p = pressure_sensors_.insert(PressureSensorMap::value_type(sensor->name_, sensor));
00480 return p.second;
00481 }
00482
00488 bool addAccelerometer(Accelerometer *accelerometer) {
00489 std::pair<AccelerometerMap::iterator, bool> p;
00490 p = accelerometers_.insert(AccelerometerMap::value_type(accelerometer->name_, accelerometer));
00491 return p.second;
00492 }
00493
00499 bool addDigitalOut(DigitalOut *digital_out) {
00500 std::pair<DigitalOutMap::iterator, bool> p;
00501 p = digital_outs_.insert(DigitalOutMap::value_type(digital_out->name_, digital_out));
00502 return p.second;
00503 }
00504
00510 bool addProjector(Projector *projector) {
00511 std::pair<ProjectorMap::iterator, bool> p;
00512 p = projectors_.insert(ProjectorMap::value_type(projector->name_, projector));
00513 return p.second;
00514 }
00515
00521 bool addAnalogIn(AnalogIn *analog_in) {
00522 std::pair<AnalogInMap::iterator, bool> p;
00523 p = analog_ins_.insert(AnalogInMap::value_type(analog_in->name_, analog_in));
00524 return p.second;
00525 }
00526
00527 ros::Time current_time_;
00528 };
00529 }
00530
00531 #endif