$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 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 00045 namespace pr2_hardware_interface{ 00046 00047 00048 class ActuatorState{ 00049 public: 00050 ActuatorState() : 00051 timestamp_(0), 00052 device_id_(0), 00053 encoder_count_(0), 00054 position_(0), 00055 encoder_velocity_(0), 00056 velocity_(0), 00057 calibration_reading_(0), 00058 calibration_rising_edge_valid_(0), 00059 calibration_falling_edge_valid_(0), 00060 last_calibration_rising_edge_(0), 00061 last_calibration_falling_edge_(0), 00062 is_enabled_(0), 00063 halted_(0), 00064 last_commanded_current_(0), 00065 last_executed_current_(0), 00066 last_measured_current_(0), 00067 last_commanded_effort_(0), 00068 last_executed_effort_(0), 00069 last_measured_effort_(0), 00070 motor_voltage_(0), 00071 num_encoder_errors_(0), 00072 zero_offset_(0) 00073 {} 00074 00082 ros::Duration sample_timestamp_; 00083 00089 double timestamp_; 00090 00091 int device_id_; 00092 00093 int encoder_count_; 00094 double position_; 00095 double encoder_velocity_; 00096 double velocity_; 00097 00098 bool calibration_reading_; 00099 bool calibration_rising_edge_valid_; 00100 bool calibration_falling_edge_valid_; 00101 double last_calibration_rising_edge_; 00102 double last_calibration_falling_edge_; 00103 00104 bool is_enabled_; 00105 bool halted_; 00106 00107 double last_commanded_current_; 00108 double last_executed_current_; 00109 double last_measured_current_; 00110 00111 double last_commanded_effort_; 00112 double last_executed_effort_; 00113 double last_measured_effort_; 00114 00115 double max_effort_; 00116 00117 double motor_voltage_; 00118 00119 int num_encoder_errors_; 00120 00121 double zero_offset_; 00122 }; 00123 00124 class ActuatorCommand 00125 { 00126 public: 00127 ActuatorCommand() : 00128 enable_(0), effort_(0) 00129 {} 00130 bool enable_; 00131 double effort_; 00132 }; 00133 00146 class Actuator 00147 { 00148 public: 00149 Actuator() {}; 00150 Actuator(std::string name) : name_(name) {} 00151 std::string name_; 00152 ActuatorState state_; 00153 ActuatorCommand command_; 00154 }; 00155 00156 class PressureSensorCommand 00157 { 00158 }; 00159 00160 class PressureSensorState 00161 { 00162 public: 00163 std::vector<uint16_t> data_; 00164 }; 00165 00176 class PressureSensor 00177 { 00178 public: 00179 std::string name_; 00180 PressureSensorState state_; 00181 PressureSensorCommand command_; 00182 }; 00183 00184 class AccelerometerCommand 00185 { 00186 public: 00187 enum {RANGE_2G=0, RANGE_4G=1, RANGE_8G=2}; 00188 enum {BANDWIDTH_1500HZ=6, BANDWIDTH_750HZ=5, BANDWIDTH_375HZ=4, BANDWIDTH_190HZ=3, BANDWIDTH_100HZ=2, BANDWIDTH_50HZ=1, BANDWIDTH_25HZ=0}; 00189 AccelerometerCommand() : range_(RANGE_2G), bandwidth_(BANDWIDTH_1500HZ) {} 00190 int range_; 00191 int bandwidth_; 00192 }; 00193 00194 class AccelerometerState 00195 { 00196 public: 00197 std::string frame_id_; 00198 std::vector<geometry_msgs::Vector3> samples_; 00199 }; 00200 00211 class Accelerometer 00212 { 00213 public: 00214 std::string name_; 00215 AccelerometerState state_; 00216 AccelerometerCommand command_; 00217 }; 00218 00219 class DigitalOutCommand 00220 { 00221 public: 00222 DigitalOutCommand() : data_(false) {} 00223 DigitalOutCommand(bool data) : data_(data) {} 00224 uint8_t data_; 00225 }; 00226 00227 class DigitalOutState 00228 { 00229 public: 00230 uint8_t data_; 00231 }; 00232 00243 class DigitalOut 00244 { 00245 public: 00246 std::string name_; 00247 DigitalOutState state_; 00248 DigitalOutCommand command_; 00249 }; 00250 00251 class ProjectorCommand 00252 { 00253 public: 00254 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) {} 00255 bool enable_; 00256 double current_; 00257 uint8_t &A_; 00258 uint8_t &B_; 00259 uint8_t &I_; 00260 uint8_t &M_; 00261 uint8_t &L0_; 00262 uint8_t &L1_; 00263 bool pulse_replicator_; 00264 }; 00265 00266 class ProjectorState 00267 { 00268 public: 00269 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) {} 00270 bool enable_; 00271 double last_commanded_current_; 00272 double last_executed_current_; 00273 double last_measured_current_; 00274 double max_current_; 00275 uint8_t &A_; 00276 uint8_t &B_; 00277 uint8_t &I_; 00278 uint8_t &M_; 00279 uint8_t &L0_; 00280 uint8_t &L1_; 00281 bool pulse_replicator_; 00282 00283 bool output_; 00284 bool rising_timestamp_valid_; 00285 bool falling_timestamp_valid_; 00286 00287 uint32_t timestamp_us_; 00288 uint32_t rising_timestamp_us_; 00289 uint32_t falling_timestamp_us_; 00290 }; 00291 00310 class Projector 00311 { 00312 public: 00313 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_) {} 00314 std::string name_; 00315 ProjectorState state_; 00316 ProjectorCommand command_; 00317 }; 00318 00319 class AnalogInCommand 00320 { 00321 }; 00322 00323 class AnalogInState 00324 { 00325 public: 00326 std::vector<double> state_; 00327 }; 00328 00329 class AnalogIn 00330 { 00331 public: 00332 std::string name_; 00333 AnalogInState state_; 00334 AnalogInCommand command_; 00335 }; 00336 00337 typedef std::map<std::string, Actuator*> ActuatorMap; 00338 typedef std::map<std::string, PressureSensor*> PressureSensorMap; 00339 typedef std::map<std::string, Accelerometer*> AccelerometerMap; 00340 typedef std::map<std::string, DigitalOut*> DigitalOutMap; 00341 typedef std::map<std::string, Projector*> ProjectorMap; 00342 typedef std::map<std::string, AnalogIn*> AnalogInMap; 00343 00368 class HardwareInterface 00369 { 00370 public: 00371 ActuatorMap actuators_; 00372 PressureSensorMap pressure_sensors_; 00373 AccelerometerMap accelerometers_; 00374 DigitalOutMap digital_outs_; 00375 ProjectorMap projectors_; 00376 AnalogInMap analog_ins_; 00382 Actuator* getActuator(const std::string &name) const { 00383 ActuatorMap::const_iterator it = actuators_.find(name); 00384 return it != actuators_.end() ? it->second : NULL; 00385 } 00386 00392 PressureSensor* getPressureSensor(const std::string &name) const { 00393 PressureSensorMap::const_iterator it = pressure_sensors_.find(name); 00394 return it != pressure_sensors_.end() ? it->second : NULL; 00395 } 00396 00402 Accelerometer* getAccelerometer(const std::string &name) const { 00403 AccelerometerMap::const_iterator it = accelerometers_.find(name); 00404 return it != accelerometers_.end() ? it->second : NULL; 00405 } 00406 00412 DigitalOut* getDigitalOut(const std::string &name) const { 00413 DigitalOutMap::const_iterator it = digital_outs_.find(name); 00414 return it != digital_outs_.end() ? it->second : NULL; 00415 } 00416 00422 Projector* getProjector(const std::string &name) const { 00423 ProjectorMap::const_iterator it = projectors_.find(name); 00424 return it != projectors_.end() ? it->second : NULL; 00425 } 00426 00432 AnalogIn* getAnalogIn(const std::string &name) const { 00433 AnalogInMap::const_iterator it = analog_ins_.find(name); 00434 return it != analog_ins_.end() ? it->second : NULL; 00435 } 00436 00442 bool addActuator(Actuator *actuator) { 00443 std::pair<ActuatorMap::iterator, bool> p; 00444 p = actuators_.insert(ActuatorMap::value_type(actuator->name_, actuator)); 00445 return p.second; 00446 } 00447 00453 bool addPressureSensor(PressureSensor *sensor) { 00454 std::pair<PressureSensorMap::iterator, bool> p; 00455 p = pressure_sensors_.insert(PressureSensorMap::value_type(sensor->name_, sensor)); 00456 return p.second; 00457 } 00458 00464 bool addAccelerometer(Accelerometer *accelerometer) { 00465 std::pair<AccelerometerMap::iterator, bool> p; 00466 p = accelerometers_.insert(AccelerometerMap::value_type(accelerometer->name_, accelerometer)); 00467 return p.second; 00468 } 00469 00475 bool addDigitalOut(DigitalOut *digital_out) { 00476 std::pair<DigitalOutMap::iterator, bool> p; 00477 p = digital_outs_.insert(DigitalOutMap::value_type(digital_out->name_, digital_out)); 00478 return p.second; 00479 } 00480 00486 bool addProjector(Projector *projector) { 00487 std::pair<ProjectorMap::iterator, bool> p; 00488 p = projectors_.insert(ProjectorMap::value_type(projector->name_, projector)); 00489 return p.second; 00490 } 00491 00497 bool addAnalogIn(AnalogIn *analog_in) { 00498 std::pair<AnalogInMap::iterator, bool> p; 00499 p = analog_ins_.insert(AnalogInMap::value_type(analog_in->name_, analog_in)); 00500 return p.second; 00501 } 00502 00503 ros::Time current_time_; 00504 }; 00505 } 00506 00507 #endif