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 std::string name_;
00239 ForceTorqueState state_;
00240 ForceTorqueCommand command_;
00241 };
00242
00243
00244 class DigitalOutCommand
00245 {
00246 public:
00247 DigitalOutCommand() : data_(false) {}
00248 DigitalOutCommand(bool data) : data_(data) {}
00249 uint8_t data_;
00250 };
00251
00252 class DigitalOutState
00253 {
00254 public:
00255 uint8_t data_;
00256 };
00257
00268 class DigitalOut
00269 {
00270 public:
00271 std::string name_;
00272 DigitalOutState state_;
00273 DigitalOutCommand command_;
00274 };
00275
00276 class ProjectorCommand
00277 {
00278 public:
00279 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) {}
00280 bool enable_;
00281 double current_;
00282 uint8_t &A_;
00283 uint8_t &B_;
00284 uint8_t &I_;
00285 uint8_t &M_;
00286 uint8_t &L0_;
00287 uint8_t &L1_;
00288 bool pulse_replicator_;
00289 };
00290
00291 class ProjectorState
00292 {
00293 public:
00294 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) {}
00295 bool enable_;
00296 double last_commanded_current_;
00297 double last_executed_current_;
00298 double last_measured_current_;
00299 double max_current_;
00300 uint8_t &A_;
00301 uint8_t &B_;
00302 uint8_t &I_;
00303 uint8_t &M_;
00304 uint8_t &L0_;
00305 uint8_t &L1_;
00306 bool pulse_replicator_;
00307
00308 bool output_;
00309 bool rising_timestamp_valid_;
00310 bool falling_timestamp_valid_;
00311
00312 uint32_t timestamp_us_;
00313 uint32_t rising_timestamp_us_;
00314 uint32_t falling_timestamp_us_;
00315 };
00316
00335 class Projector
00336 {
00337 public:
00338 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_) {}
00339 std::string name_;
00340 ProjectorState state_;
00341 ProjectorCommand command_;
00342 };
00343
00344 class AnalogInCommand
00345 {
00346 };
00347
00348 class AnalogInState
00349 {
00350 public:
00351 std::vector<double> state_;
00352 };
00353
00354 class AnalogIn
00355 {
00356 public:
00357 std::string name_;
00358 AnalogInState state_;
00359 AnalogInCommand command_;
00360 };
00361
00368 class CustomHW
00369 {
00370 public:
00371 std::string name_;
00372 };
00373
00374 typedef std::map<std::string, Actuator*> ActuatorMap;
00375 typedef std::map<std::string, PressureSensor*> PressureSensorMap;
00376 typedef std::map<std::string, Accelerometer*> AccelerometerMap;
00377 typedef std::map<std::string, ForceTorque*> ForceTorqueMap;
00378 typedef std::map<std::string, DigitalOut*> DigitalOutMap;
00379 typedef std::map<std::string, Projector*> ProjectorMap;
00380 typedef std::map<std::string, AnalogIn*> AnalogInMap;
00381 typedef std::map<std::string, CustomHW*> CustomHWMap;
00382
00408 class HardwareInterface
00409 {
00410 public:
00411 ActuatorMap actuators_;
00412 PressureSensorMap pressure_sensors_;
00413 AccelerometerMap accelerometers_;
00414 ForceTorqueMap ft_sensors_;
00415 DigitalOutMap digital_outs_;
00416 ProjectorMap projectors_;
00417 AnalogInMap analog_ins_;
00418 CustomHWMap custom_hws_;
00424 Actuator* getActuator(const std::string &name) const {
00425 ActuatorMap::const_iterator it = actuators_.find(name);
00426 return it != actuators_.end() ? it->second : NULL;
00427 }
00428
00434 PressureSensor* getPressureSensor(const std::string &name) const {
00435 PressureSensorMap::const_iterator it = pressure_sensors_.find(name);
00436 return it != pressure_sensors_.end() ? it->second : NULL;
00437 }
00438
00444 Accelerometer* getAccelerometer(const std::string &name) const {
00445 AccelerometerMap::const_iterator it = accelerometers_.find(name);
00446 return it != accelerometers_.end() ? it->second : NULL;
00447 }
00448
00454 ForceTorque* getForceTorque(const std::string &name) const {
00455 ForceTorqueMap::const_iterator it = ft_sensors_.find(name);
00456 return it != ft_sensors_.end() ? it->second : NULL;
00457 }
00458
00464 DigitalOut* getDigitalOut(const std::string &name) const {
00465 DigitalOutMap::const_iterator it = digital_outs_.find(name);
00466 return it != digital_outs_.end() ? it->second : NULL;
00467 }
00468
00474 Projector* getProjector(const std::string &name) const {
00475 ProjectorMap::const_iterator it = projectors_.find(name);
00476 return it != projectors_.end() ? it->second : NULL;
00477 }
00478
00484 AnalogIn* getAnalogIn(const std::string &name) const {
00485 AnalogInMap::const_iterator it = analog_ins_.find(name);
00486 return it != analog_ins_.end() ? it->second : NULL;
00487 }
00488
00494 CustomHW* getCustomHW(const std::string &name) const {
00495 CustomHWMap::const_iterator it = custom_hws_.find(name);
00496 return it != custom_hws_.end() ? it->second : NULL;
00497 }
00498
00504 bool addActuator(Actuator *actuator) {
00505 std::pair<ActuatorMap::iterator, bool> p;
00506 p = actuators_.insert(ActuatorMap::value_type(actuator->name_, actuator));
00507 return p.second;
00508 }
00509
00515 bool addPressureSensor(PressureSensor *sensor) {
00516 std::pair<PressureSensorMap::iterator, bool> p;
00517 p = pressure_sensors_.insert(PressureSensorMap::value_type(sensor->name_, sensor));
00518 return p.second;
00519 }
00520
00526 bool addAccelerometer(Accelerometer *accelerometer) {
00527 std::pair<AccelerometerMap::iterator, bool> p;
00528 p = accelerometers_.insert(AccelerometerMap::value_type(accelerometer->name_, accelerometer));
00529 return p.second;
00530 }
00531
00537 bool addForceTorque(ForceTorque *forcetorque) {
00538 std::pair<ForceTorqueMap::iterator, bool> p;
00539 p = ft_sensors_.insert(ForceTorqueMap::value_type(forcetorque->name_, forcetorque));
00540 return p.second;
00541 }
00542
00548 bool addDigitalOut(DigitalOut *digital_out) {
00549 std::pair<DigitalOutMap::iterator, bool> p;
00550 p = digital_outs_.insert(DigitalOutMap::value_type(digital_out->name_, digital_out));
00551 return p.second;
00552 }
00553
00559 bool addProjector(Projector *projector) {
00560 std::pair<ProjectorMap::iterator, bool> p;
00561 p = projectors_.insert(ProjectorMap::value_type(projector->name_, projector));
00562 return p.second;
00563 }
00564
00570 bool addAnalogIn(AnalogIn *analog_in) {
00571 std::pair<AnalogInMap::iterator, bool> p;
00572 p = analog_ins_.insert(AnalogInMap::value_type(analog_in->name_, analog_in));
00573 return p.second;
00574 }
00575
00581 bool addCustomHW(CustomHW *custom_hw) {
00582 std::pair<CustomHWMap::iterator, bool> p;
00583 p = custom_hws_.insert(CustomHWMap::value_type(custom_hw->name_, custom_hw));
00584 return p.second;
00585 }
00586
00587 ros::Time current_time_;
00588 };
00589 }
00590
00591 #endif